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I. INTRODUCTION 


A. "TARGET TRACKING 

The detection,location, and tracking of targets plays a 
critical role in many aspects of military operations. The 
ability to accurately track and predict the movement of 
aircraft, anti-ship missiles, and torpedoes in military 
operations can not be overstated. This report investigates 
the use of multiple sensors and Kalman filtering in Fusion 
Tracking. 

The Kalman filter has been in use for over three decades 
іп aircraft avionics, radar, sonar, and multiple control 
problems. This investigation covers a Kalman filter in 
tracking a target by bearing observations and an additional 


problem using bearing, range, and frequency observations. 


II. THE KALMAN FILTER 


The Kalman filter estimates the state of a linear and 
time-invariant (LTI) system given a set of known inputs to the 


system and a set of measurements. 


A. DEFINITION OF TERMS 

The terms used in this chapter and Appendix B, are defined 
in Table (2.1). Kalman filter system dynamics utilize state 
space representation, therefore matrix dimensions are given. 


TABLE 2.1: Definition of Terms 





Dimension Code Symbol 
Error covariance matrix: nxn P 
Estimate error: п х 1 
Expected value of error: n xw 
Identity matrix: nxn eye(n) I 
Kalman gain matrix: G(k) ` 'G, 
Measurement matrix: її хХ 1 Н Н 
Observation: theta(k) 2, 
Observation noise: 1х1 Е V, 
Residual: 1х1 г, 
State estimate: п х1 ХКК 
State excitation nolse: Q w, 
State transition matrix: nxn Phi 
System state: n x 1 Х X 
Transpose: x ' Хх, 


Å SSS SS SSS SSS SSS SS 
Appendix A defines the terms used in the Kalman filter 
designed around the torpedo tracking problem. This program is 


discussed further in Chapter IV. à 


B. EQUATIONS AND INITIALIZATION 
The background given here coincides with that of (Ref 1]. 


The equations for the filter used in this program are listed 


below. 
сос Om [U P Нак] Б (2.1) 
ТЫ СН] Р, (222) 
Prive = 06PA07*0, (2.3) 
Хак = СЕЕ ЕЭ А (2.4) 
Е Фл (2.5) 
ZkNk-1 š AX ka (276) 


Terms with a single time subscript (i.e., x,) refer to the 
value of the term at that time. Those terms with dual 
subscripts (í.e., Paeng) refer to the value of the term at a 
future time given the present states. 

The Kalman filter requires initialization and the proper 


choice of initialization is important. Тһе value of X,,., is 


initially set to zero and the following steps are then 


evaluated in the recursion algorithm. 


* Use Equation 2.1 to calculate the Kalman gain. 

* Use Equation 2.2 and 2.3 to evaluate the values of the 
covariance of estimate error matrix for the next 
iteration. 


* Use Equation 2.4 to find the state estimates. 


* Use Equation 2.5, given the current state estimates to 
project ahead for use in the next iteration. 


In the program, the bearing theta is simulated and sampled by 


the filter as if they were taken from a sensor. 


ІІІ. THE MODEL 


In thís chapter, we introduce the mathematical and 
physical model for our first tracking problem. We start by 
presenting the physical relationship between the target and 
the observer. After the track has been generated, we use the 
Kalman filter to verify that the mathematical model 15 


tracking the physical model. 


А.  PHYSICAL SYSTEMS 

The first system used in this investigation consists of a 
single observer and a single target. For the generation of 
the target's track, a two-dimensional Cartesian coordinate 
system with the observer at the origin is used. When 
referring to Figure 3.1, this x-y grid can be thought of as 
looking down on a north-south/east-west grid. The target is 
free to move, unrestricted, throughout the coordinate space 
while the observer is stationary and remains at the origin. 

The information received by the observer consists of 
bearing to the target. Therefore, the state space estimate 


appears as: 


Ж -79 (3.1) 


Observer 





Figure 3.1: First Physical System 


These bearings are to be consistent with measurements supplied 
by a phased array radar, in receive only mode, and most types 
of infra-red scanners used today. The measurements are taken 
from the radar 100 times per second, while the infra-red is 


sampled every second. 


B. MODEL TEST 

It is critical that the recursive program receive its 
initial states and observations in the proper order. 
Therefore, че will examine the Kalman gains for a 
verification. The MATLAB programs MISSILE1.M and MISSILE2.M 
used are found in Appendix B. For the purpose of 
verification, we will assume no noise with the covariance of 
measurement noise and covariance of excitation equal to zero. 


If the filter is working properly, the Kalman gains for the 


first sample should have the position vector G(1,1) equal to 
one. The velocity and acceleration vectors, G(2,1) and G(3,1) 
respectively, should equal zero (see Figures 3.9 through 
3.11). At the second sample, G(1,2) is given by Х(2) - 2(2), 
G(2,2) is given by (1/T)[z(2)-z(1)], and G(3,n) should remain 
zero. For the third sample, G(3,3) should take on a value 
given by 


ОК КП а а еј 2 (1) 1). (3.2) 


For this simulation the target has v, = 1 km/sec as shown 
in Figure 3.3. Figures 3.4 and 3.5 show the distance traveled 
and change in bearing over one second. The figure showing the 
change in bearing might be deceptive, as it appears to be a 
straight line. In fact it is a curved line that reaches its 
maximum rate of change when the observer is perpendicular to 
the target's path. Figures 3.6 through 3.8 show the state 
estimates of the Kalman filter, that gives us a comparison to 
the actual change in bearing. 

Figures 3.9 through 3.11 are the Kalman gains. However, 
the covariance of measurement noise is 0.001. This avoids a 
divide by zero in the Kalman gain equations. This will have 


a minimal effect on the results. 





Torgets 








Figure 3.4: Observed change in bearing 


Kolman fliter emtirnmaote of bearing 





Figure 3.5: State estimate of bearing 


Estimate of Angular rate 


40 БО eo 
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Figure 3.6: State estimate of change in rate 


Eatimotea of angular acceleratian 


40 ео во 


CSamples taken) 


Figure 3.7: State estimate of angular acceleration 








Figure 3.8: Kalman filter gain vector G(1,k) 





Figure 3.9: Kalman filter gain vector G(2,k) 





Figure 3.10: Kalman filter gain vector G(3,k) 
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IV. THE KALMAN FILTER WITH MANEUVER DETECTION 


7 


— 


A. PROGRAMS 
This m is devoted to an explanation of the 
components and logic used for the programs in Appendix C. 
TORPMAN.FOR uses the combined work from [Ref 1] and Raymond M. 
Alfaro. In order to run the program, follow this procedure. 
e Run TORPSCUR.M for the s-curve or TRACKSIM.M for the 
radial track as in [Ref 1]. 


* GENFILES.M converts the track to a data format. 


• POSCONV.FOR converts the MATLAB (ASCII) to FORTRAN 
(keyport) input. 


e Run OBSDATA.FOR to covert position, velocity, 
acceleration, time, and roll data files to a single 
observation file OBSERVAT.DAT 
° TORPMAN.FOR is the Kalman filter and may be analyzed by 
using ANALYZE.M . 
B. STATE SPACE AND OBSERVATION MATRICES 

As stated previously, this problem utilizes the Cartesian 
coordinate system. The state space is made up of position, 
velocity, and acceleration in the x, y, and Z vector space, 


where the dot denotes a time derivative. 


X= [x X Z y y y z Z 2” (4.1) 
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t? 

16 E оо о ооо 

Ол 0 O0 ООО 

070 60708 TOTT mi 

t? 
00 ` 0 1 L E 00 0 
Е (4.2) 

00. 0 Отта а 

00 0 0 07779070770 
t? 

ооо оо окт ES 

0 0 ОЎО OFO T XE 

0 0 0 00 0 00] 


The observation measurements for our system consists of 
position and velocity measurements with or without noise 
added. Observation measurements are defined in the following 


manner. 


(4.3) 


N 
li 
NN SSS XX 


We are assuming the acceleration information is not available. 
However, if it were possible to get the observation, the 
filter would take advantage of the opportunity to improve its 
estimate. We will also assume that some observation may have 
missing position and velocity data. This will be discussed in 


depth later in the chapter. 
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1. Physical Model 
For the system as shown in Figure 4.1, the Cartesian 
coordinate system is used. This implies an array of sensor 
generate the range measurements. Again the target is free to 
move throughout the coordinate space. However, the graphic 
information displays only X-Y coordinates. Frequency 
information is also utilized in the velocity estimate by 


estimating the turn counts. 


Observer 





Figure 4.1: Second Physical System 


C. WEIGHTED COVARIANCE MATRIX 


The covariance of excitation error matrix, Q(k), is 
considerably more complex. The matrix can be weighted or 
manipulated when a maneuver is detected. In this new 


arrangement, the x, y, and Z coordinates are adjusted 
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independently of the others. This matrix was derived in [Ref 


2, pp. 36-42] and is given below. 


t? 
— w 0 0 0 0 0 0 0 0 
36 os 
tê 
Qm ш 0 0 0 0 0 0 
4 
0 0 t?w. 0 0 0 0 0 0 
о - 0 о 0 
0 — w" 0 
a6 Y 
Q 0 0.00 9 S k о о |“ 
4 У 
0 0 0 0 0 ме, 0 0 0 
0 0 0 0 0 0 U^ 0 
— w 0 
36. 
t4 
0 0 0 0 0 0 MEME EO 
4 
0 0 0 0 0 0 0 0  t?w| 
The expression for distance is 
te 
DC ( ) 
where 
t = sample time interval, 
w = weighting factor, and 
s = distance travelled in one sample interval. 


А good example of finding w ls given in [Ref 1, pp. 22]. One 
should be reminded, when solving for w, it is the worst case 


and a smaller weight is more robust. 
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1. Error Ellipsoids 
Error ellipsoids are useful in visualizing the margins 
of error. The state value should lie within a certain region 
surrounding the estimate. This uncertainty is expressed in 
the covariance of error matrix P,,,. 
The position components submatrix of the error covariance 
matrix P,,, is defined as 
2 


E s p -| var x _ E ox Oxy 


APL Pag GOV X,Y) var y 


(4.6) 
О 


Oxy 
The diagonal terms P, and P, of the covariance matrix 
represents the variances of the estimate in the x and y 
positions respectfully. ен = a nonnegative definite 
matrix and the random Gaussian noise processes w(k) and v(k) 
are independent, a surface of constant probability density can 


be produced. To elaborate on what is desired, Figure 4.2 is 


provided. 
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Ох 


Figure 4.2: Dimensions for the error ellipse 


a. Matrix Method 


From the density function 


е-х"Рх, (4.7) 
we specify the level surface about the origin by 
Хара ТЕ (4.8) 


From numerical analyses [Ref 3, pp. 164], there is а unique 
upper triangular matrix R such that 
P = RTR (4.3) 
Moreover, this matrix can be stably computed without pivoting. 
This result, however, ls equivalent to stating that there is 
a corresponding unique triangular matrix L(=R') such that 
DECEM (4.10) 
In terms of the matrix, our basic problem 4.8 becomes 


xT(LLT)`1lx = 1 (4.11) 


However, elementary matrix properties imply that 
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nC Ep cy -LRS xT(LT) Tr ls 
OL Ey CT SEC) i212) 
(eL a [te oct 


where | l, represents the usual Euclidean norm. But, this last 


— 


— 


equality implies that 
х7ріх= 1 == |? х|{, =1 ; (4.13) 
Therefore, the basic problem 4.10 is eqüivalent to 
IZ xp ! (4.14) 
This equation is solvable if and only if, 
Lx -y for some vector y such that |у|, = 1 (4. 15) 
But, 
ШЕ у е гу ! (4.16) 
Therefore, we conclude that the desired error ellipsoid is 
specified by 
{ x|x=Ly , |у|,=1) (42 7) 
b. Reaction to a maneuver 
The effects of maneuver detection are seen in 
Figure 4.3. As the filter detects an acceleration in one 
direction, the uncertainty of the position increases in that 
dimension. The ellipsoids can be used to verify the filters 
ability to predict the movements of the target. The program 
ELLIPSE.M that generates the ellipsoids can be found in 
Appendix B. 
c. Initialization check 
The program S ELLIPSE.M contained in Appendix C 


give us the same visual verification of the filter using the 
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Figure 4.3: The effect of maneuver detection on the 
covarlance of error 


same idea. By using the error covariance matrix P we can 
obtain a visual insight into the performance of the filter. 
Figure 4.4 shows the exponential decay of the probability of 


error. 


D. THE OBSERVATION MATRIX - H 
The logic for the observation matrix starts with the 
manipulation of data in OBSDATA.FOR, in Appendix C. After 


receiving an observation, the data is manipulated to obtain a 
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Figure 4.4: The correct response for the covariance of 
error ellipsoids 


state space vector with position, velocity, and acceleration. 
Based on which state variables are actually present in the 
observation, a unique binary signal is generated. Take the 
coefficient of the power 


1 if x, 1s present in the observation 


2171 = { ; - 
О 1f x, ls not present 


The number is stored with the position vector (x) as low bit. 


For example: 


звс = ла = | о | о | ој ој о| з | о | 1 |: 


1° 


The binary number implies that x, xX and y make up the 
observation. This allows subsequent decoding of which 
components are present by a combination of divide by 2 and 
modulo opus. 

The matrix H, can be produced for any combination of 
measurements. Given the same statistical characteristics as 
the actual data, lt is possible to have missing data and 
multiple returns from adjacent arrays. This filter has the 
ability to continue when pieces of data are not received, or 
when the two sensors are operating at slightly different 
frequencies. The scheme used here helps in this regard in 


that it is more robust. 


20 


V. RESULTS 


A. BASELINE INFORMATION 

The s-curve data is run through the filter from [Ref 1, 
pp. 61]. This information will now be used in determining 
improvements in performance. The knowledge that the filter 
performed best with the excitation matrix weighted to 1500, 
is utilized in the production of Figure 5.1. The 


Time vs, Error in Position — S Curve 


Position Error (ft) 





20 25 


Time (sec) 


Figure 5.1: X and Y smoothed estimate error with old 
maneuver detection scheme 


performance of the filter can be improved in this noiseless 
environment by increasing the figure for the weighing from 
1500. However, as shown in [Ref 1], the filter's performance 


in a noisy environment would be degraded. 
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For a better idea of what is causing the errors, Figure 
5.2 shows when the target is maneuvering. Looking at where 
the maneuvers occur, coincides with the disturbance in 


tracking. 


X Acceleration vs. Time 


N 
O 


Acceleration (ft/sec~2) 


20 25 


Time (sec) 


Y Acceleration vs. Time 


Acceleration (ft/sec~2) 


20 25 


Time (sec) 





Flgure 5.2: Target's acceleration 


B. MODIFICATIONS 

The filter's maneuver detection scheme and observation 
measurement matrix, (H), are now as discussed in the previous 
chapter. This filter is modified in one other respect. This 
filter manipulates the excitation matrix when a maneuver is 
detected, not after the maneuver is detected. The purpose of 


estimating the acceleration is to be able to sense the 
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maneuver simultaneously with the onset of the maneuver. "None 
of the time lag associated with the residual method is 
experienced." [Ref 1] The results are readily seen in Figure 
DES. SO this is with no noise and the Q matrix is 
weighted at unity. A view of the targets path is provided in 


Figure 5.4. 


Forward (==) ond Bockword (=.) X Error vs, Time 


X—Error Magnitude 
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Time (sec) 


Forward (==) ond Backword (—.) Y Error vs. 


Y—Error Magnitude 


Time (sec) 





Figure 5.3: X and Y error for the s-curve using the new 
filter; weight is set at unity 


Note that the smoothing in backward filter causes the 


larger of the errors in a noiseless environment. The more the 
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1590 





x— oxis 
Figure 5.4: The targets track in feet 
excitation matrix ls weighted the less the errors become. 
But, again, this does not hold true with noise in the 


environment and the trade offs need to be evaluated. 


C. EVALUATION FOR OPTIMAL PERFORMANCE 

In order to evaluate the differences in the two filters, 
we must first establish the optimal performance for this new 
filter. Multiple tracks are created to establish a value for 
the weight and to gather enough data to reduce any statistical 
errors. 

As discussed in [Ref 1, pp. 31] Gaussian noise is added to 
the track with a distribution variances of 15 square feet, 
0.01 square yards/second, and 0.03 square feet/second squared 


for position, velocity, and acceleration respectively, with 
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zero mean. A figure of such a track is provided in Figure 


5.5. 


x (—) and xrandom (-—) 


1000 1500 2000 





х--(хі8 


Figure 5.5: Target track with noise added 


Trial and error is used to find the optimal performance of 
the filter. The numbers seen in Table 5.1 are the mean radial 
distance errors obtained from the program in [Ref 1, pp. 100]. 
Table 5.1 shows a few of the multiple iterations of 
combinations used in choosing the optimal weights. The 
maximum weight (WTMAX) and minimum weight (WTMIN) are chosen 
to be 100 and 0.1 respectively. This choice offers a half 
foot reduction in mean radial distance error in a relatively 


quiet environment, while giving up a little less than a half 


2b 


foot in this noise filled environment. Figure 5.6 shows how 


the filter handles a noisy track for one run. 


TABLE 5.1: Table of weights 


Maximum Weight 
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Flgure 5.6: Error for noisy track 


D. COMPARISONS 


Both filters, using all of their best features, are used 
on a track with approximately 20 feet of mean radial distance 
error. Figure 5.7 provides a visual comparison as to the 
capability of each filter in the same figure. The mean radial 
distance errors in the upper left hand corner show the 


difference in performances of the old and new filters. 


27 


Old Track - 4,82 
Мем Тгоск:- 0.2491 


ТА РАНЕ орла 
x : | | -- Old Trock 
-. New Tróck 


500 1000 1500 2000 2500 


X - Axis 


Figure 5.7: A comparison between mean radial distance of the 
old and new fllter 





For the circular maneuver presented in [Ref 1,pp. 36], we 
Will look at the best result for the old filter with optimal 
settings. In this track the torpedo makes 360 and 270 plus 
degree turns. The old mean radial distance error was 5.951 
In Figure 5.8, the mean radial distance errors are in the 
upper left hand corner. The old track is the "raw" 
observation while the new track is the smoothed filter 
estimate of the true track. The mean radial distance error 


show over a 50% improvement with an error of 2.165 
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Old Track - 18.73 
New Track = 2.165 


ТОЙЫ" 
-- (Old Trock 
=, New Track 





Figure 5.8: Filter output with WTMIN=0.1 and WTMAX=100.0 
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VI. CONCLUSIONS AND RECOMMENDATIONS 


The objective was to investigate uses of fusion in 
tracking problems. In the process, there were several items 
that bore out special investigation. 

The program for torpedo track reconstruction now works in 
three dimensions and its overall performance improved. One 
major improvement was the creation of the OBSDATA.FOR program 
апа its subsequent improvement on the observation or 
measurement matrix. This revamping of the program and its 
logic has improved its overall characteristic another 50 to 
80%. 

The tracking of targets by electromagnetic and infra-red 
sensors needs to be pursued. The sensors in (Ref 4], all have 
the capability of tracking fast moving targets. The 
preliminary results look good for three polar coordinate 
filters, all working in parallel, to track air traffic 
passively. If there were one filter per sensor, and one 
filter to fuse and triangulate bearings using a conversion 
vector like in [Ref 4, pp.4-4], then vessels would be able to 
track traffic passively іп conditions of restricted 


electromagnetic emissions. 
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APPENDIX A: DEFINITION OF TERMS FOR USE IN TORPMAN.FOR 


The terms and characters defined here are for easy access 
to those trying to comprehend the program in Appendix C. The 
logic for the program is discussed in more depth in Chapter 
EV. 


TABLE A.1: Terms used in TORPMAN.FOR 





$3DTEMP This is a direct access file for storing 
values of the error covariance matrix (Рак) • 
The character associated with this file is 
PKKS3D. 


SM1TEMP This is a direct access file for storing 


values of PKKM1S or the smoothed D 


ADD A subroutine which adds two input matrices. 
ACCTHRSH The acceleration threshold is initialized to 
5 feet/second. This is for use in the 


maneuver detection program, MANDET, and may 
be changed within the SETTHRSH subroutine. 


CHANGE This is a subroutine which allows the user b 
change the weight (WTMIN or WTMAX) and error 
covariance matrix at time К minis 1 (РККМ1). 
The program is therefore altered in behavior 
without having to compile. 


H The measurement matrix 1s discussed in more 
depth in chapter V. 


NDIMEN The number of dimensions used are 1, 2, or 3 
D. 

NUMSTA This is the number of states in the vector 
space. 

NSTMAT This is used in FORTRAN memory allocation fr 


one time step. 


3 4 


NOBVEC 


OBSERVAT. DAT 


PHIDEL 


PKK 


PKKM1 


PKKM1S 


PKKOUT.DAT 


ROLTHRSH 


SCR 


TMPV1 & 2 


TEMP1, 


WIMIN 


ХКК 


2, 


& 


This allocates an array large enough for the 
observed data set. 


The observed data base. 
This subroutine builds the Phi and Del matrix. 
The error covariance matrix (P). 


The error covariance matrix (P,,,.,), which is 
at time minus one. 


This is the smoothed PKKM1 from the backward 
Kalman filter. 


An output file is created containing the 
information for error ellipse in the x and у 
plane. 


State excitation noise matrix. 
Observation noise matrix. 


The roll threshold is initialized to 5 
degrees/second. This can be changed with tte 
SETTHRSH subroutine. 


An integer, when read as a binary number, 
reveals the make up of the observation. 


A temporary storage space for characters as 
they change value from k/k-1 to k/k to k+1/k 
= k/k-1 . 


A temporary storage space for characters as 
they change value from k/k-1 to k/k to k*1/k 
= k/k-1 


This is the maximum weight used upon the 
detection of a maneuver. The value is preset 
to 20,000 but can be altered in the CHANGE 
subroutine. 


This is the minimum weight used in the 
program. Which insures that the Kalman gains 
do not approach zero if the target is not 
maneuvering. ` 


The state estimate (%,,,). 0) 
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XKKM1 


XKKFWD. DAT 


XKKS 


XKSMOOTH. DAT 
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The state estimate (%,,,.,), which is at 
time minis one. 


The state estimates of the Kalman filter are 
stored in this data file. 


Observations are stored in this matrix, 
including acceleration obtained from AV. It 
is then used for the state estimate and 
smoothed state estimate as it passes through 
forward and backward Kalman filter. 


The state estimates of backward filtering 
(XKKS) are stored in this file. 


Stores the observations. 
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APPENDIX B: MATLAB PROGRAMS 


А. ANALYZE.M 


$analyze.m 

$ This M-file compares the output of a Kalman Filter Run 
$ with the true x, y, and z positions. 
clg 

'del data\six.met 

! са data 

load tracknum.dat 

tracknum = fix(tracknum) ; 

load trueposn. 

load xkkfwd.dat 

load xksmooth.dat 

% 

errfwd-xkkfwd(:,2:4)-trueposn ; 
errbwd-xksmooth(:,2:4)-trueposn ; 


$ 

eval( [ '! del filtr',num2str(tracknum),'.met' ]) 
subplot(211) ; 

plot (xkkfwd(:,1),errfwd(:,1),'--g',.. 


xkkfwd(:,1), errbwd(:,1),'!'-.w"' ) 
xlabel('Time (sec)');ylabel('X-Error Magnitude' ); 
title('Forward (--) and Backward (-.) X Error vs. Time'); 
% 
plot(xkkfwd(:,;1)$jerEbEfwd( 2 2) UU 
xkkfwd(:,1),errbwd(:,2),'=.w') 
xlabel('Time (sec)');ylabel('Y-Error Magnitude' ); 
title('Forward (--) and Backward (-.) Y Error vs. Time'); 
eval( { 'meta filtr',num2str(tracknum) ]) 
meta six 
pause 
% 
Gig 
subplot(211) 
plot(xkkfwd(:,1),errfwd(:,3),'=-g',.. 
xkkfwd(:,1),errbwd(:,3),'-.w') 
xlabel('Time (sec)');ylabel('Z-Error Magnitude' ); 
title('Forward (--) and Backward (-.) 2 Error vs. Time'); 
$meta 
pause 
C1g 
$ 
eval("! edan) 
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B. ELLIPSE.M 


This program will plot an error ellipsoid once ever six 
observations. 


!del pic2.met 

load c:\matlab\wiseman\output2.dat 

load c:\matlab\wiseman\truepos2.dat 

load c:\matlab\wiseman\pkkout.dat 
axis('square') 
%5%%%%5%%%%%%%%%%%%%%%%%%%%5%5%%%%5%%5%%5%%5%% 
% plot path'and estimate of path % 
қонамын ЗАЛАЛ 
plot(output2(:,1),output2(:,2),' 

NEU E 301 
xlabel('X - Position (ft)');ylabel('Y - Position (ft)'") 
title('Trajectory - S Curve!) 
gtext(' True track!) 
gtext('+ + * Estimate!) 
gtext('O Error ellipsoid') 
gtext('20 times larger') 
%%%5%%%%%%%%%%%%%%%%%%%%%%%5%%%%%%%%%%%%%%%%%%%%%%%%% 
$ generate a uniformly spaced set of points % 
%%%%%5%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%%%%%9%%% 
delt=-1:.05:1; 
delt-pi*delt; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%%%5%%%%59%9%% 
$ generate a (2xm) set of vectors on the unit cir 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
y-[cos(delt);sin(delt)]; 
shift-diag(ones(41)); 
shift-shift'; 
hold on 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% The function chol, to compute the Choleski % 
% decomposition, is done "backwards" in MATLAB. It % 
$ makes an upper triangular matrix R, not L. So take % 
% the transpose. % 

% 
% 
% 





Q oe 
oo k-2 oe 
ov (D 


$ Ап array of x-vectors (2xm) has been created that 
% form the error ellipsoid. 
%%55%5%%%%%%5%%%%%%%%%%%%%%%%%%%%%%%%%5%%%%%5%%%%5%5%%5%5%5%% 
for k=6:6:max(size(output2)), 

P=pkkout (k*2-1:k*2,1:2); 

s-chol(P)'; 

х=ѕ*у; 

plot(x(1,:)*20+output2(k,1)*shift, 

x(2,:)*20+output2(k,2)*shift) 

end 
hold off 
axis('normal!) 
meta pic2 
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C. GENFILES.M 


THIS PROGRAM CONVERTS SIMULATED TORPEDO TRACKS GENERATED BY 
THE MATLAB PROGRAM TORPGEN.M INTO FILES SUITABLE FOR INPUT 
TO THE FORTRAN PREPROCESSOR  OBSDATA.FOR. 

AFTER CONVERSION INTO STANDARD INPUT FORMAT BY OBSDATA.FOR, 
THE DATA WILL BE TESTED AGAINST THE KALMAN FILTERING PROGRAM 
TORPMAN.FOR, WHICH IS DESIGNED TO RECONSTRUCT TORPEDO TRACKS 
GENERATED AT THE UNDERSEA RANGE AT KEYPORT, WA. 


де де де де де de де де де де 


%%%%%%% Prompt User for Desired Type and Run %%3%%%%%%%%%%%% 
1111111 Number for Data File %%5%%%%%%%%%%% 
% 

Шел Ес 

disp(' Select Type of Simulated Track to Process ' ) 

disp(' ') 

disp(' 1. True Track (w/o pseudo-random noise added)' ) 
disp(' 2. Track with Pseudo-Random Noise Added" ) 

disp(' (Any other value will abort processing) ') 

disp(' ') 

ntrktyp = input(' Selection: ') ; 

if ntrktyp <= 0 ; return ; end 

if ntrktyp > 2 ; return ; end 

% 


disp(' '); 
trutrk - input(['!Enter the Sequence Number of the True Track 
File: “тулж: 
obsfil = ['true']; 
obstrk = trutrk; 
1f ntrktyp == 
disp(' '); 
obsfil = ['tst']; 
obstrk - input(('Enter the Sequence Number of the Simulated 
Track File: ']J) ; 
end 
% 


%%%%%%%%%% Іпрчие the Desired Files and Produce %%%%%%%%%% 
%%%%%%%%%% Standard Output Files That Will Run %%%%%%%%%% 
%%5%%%%%%% with Program POSCONV.FOR %%%%%%%%3%% 
% trueposf = ['truep',int2str(trutrk) ] ; 

deltatf = ['delt',int2str(trutrk) ] ; 

eval( ( 'load data\',trueposf ]) 

eval( [ ‘load data\',deltatf ]) 


save data\trueposn x /ascii 
save data\deltsimu deltat  /ascii 


positionf 
velocityf 


[ obsfil,'p',int2str(obStER) M) 
( obsfil,'v',int2str(obstrk) ] 


`. 
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acceleraf = [ obsfil,'a',int2str(obstrk) |; 
еуа1( [ 'load dataN',positionf |) 
еуа1( [ 'load data\',velocityf |) 
eval( [ 'load dataN',acceleraf |) 


save dataNtracknum.dat obstrk /авсіі 


save dataNsimulpos x /авсіі 
save dataNsimulvel у /авсіі 
save dataNsimulacc 3 /авсіі 


37 


D. MISSILE1.M 


MISSILE1.M 

This program was developed for tracking a target moving at 
approximately mock three. The problem is set up for the poler 
coordinate system. 


dt120.01; 

kmax-100; 

%М15511е 

Speed-1; Фарргох. 3.3 time the speed of sound 
xrange=0; $km 

yrange-20; % km 


x=zeros(4,kmax) ; 

х(:,1)-(хгапде 3*speed/5 yrange -4*speed/5]'; 
theta-zeros(1,kmax); 
theta(1,1)=atan2(x(1,1),x(3,1)); 
time=zeros(1,kmax) ; 

А-(01 0 0:0 0 0 0:0 0 0 1;0 O O Oj; 

В-(0 071 0;0 0;0 1); 

(phi,del}=c2da(A,B,datl); 


Filter 

К=0.001; 

Q=0; 

A={0 1 0;0. 0 i;:0 0 O|]; 

В=[0 0 1]'; 5 

H=[1 O 0]; 

[Phi,Del]sc2d(A,B,dt1); 

G-zeros(3,kmax); 

XKK-Zeros(3,kmax); 

XKKM1=zeros(3,kmax) ; 

PKKM1=eye (3) *1e7; 

for k=1:kmax 
G(:,kK)=PKKM1*H'! * (H* PKKM1*H'+R) - (-1) ; 
PKK=(eye(3) -G(: ,kK) *H) *PKKM1; 
PKKM1=Phi*PKK*Phi'+Q; 
XKK(:,k)=XKKM1(:,k)+G(:,k) *(theta(k)-XKKM1(1,k)); 
XKKM1(: ,k+1)=Phi*XKK(:,k); 
х(:,К+1)=рћахх (: ,К); 
theta (kt+1)=atan2(x(1,k+1),x(3,kK+1)); 
time (k+1)=time(k)+dt1; 

end 

plot(x(1,:),x(3,:)), ,grid,ylabel('(Y in (km))') 

tltle('Targets track'),xlabel('(X in (Km))') > 

meta testl 

plot (time, theta*180/pi) ,grid,title('Bearing to thé target') 

xlabel('(Sec)'),ylabel('Degrees') 
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meta test2 

plot(time(1,1:kmax) ,XKK(1,:)*180/pi) ,grid 
title('Kalman filter estimate of bearing') 
xlabel('(Sec)'),ylabel('Degrees') 

meta test3 

look=10; 

plot (time(1,1:look) ,G(1,1:look)) ,title('G(1,:)') 
xlabel('(Sec) ') 

meta test6 

plot (time(1,1:look) ,G(2,1:look)) ,title('G(2,:)") 
xlabel('(Sec) ') 

meta test7 

plot (time(1,1: look) ,G(3,1:look)),title('G(3,:)') 
xlabel('(Sec)'!') 

meta test8 

plot(XKK(2,:)),title('Estimate of Angular rate!) 
xlabel('(Samples taken)!') 

meta test4 

plot(XKK(3,:)),title('Estimate of angular acceleration!) 
xlabel('(Samples taken) ') 

meta test5 
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E.  MISSILE2.M 


This program was developed for tracking a target using fusion. 
The two sensors are located at the origin. 


dt120.01; 
dt221; 
kmax=3000; 


Missile 

speed=1; approx. 3.3 time the speed of sound 
xrange=0; km 

yrange=20; $ Кт 

X(:,1)=[xrange 3*speed/5 yrange -4*speed/5]'; 
theta(1,:)=atan2(x(1,1),x(3,1))7 

А=[0 1 оО 0:0 0 0 0:0 0 0 1;0 0 O Oj; 

В=[0 0;1 0;0 0;0 1]; 

[phi,del]=c2d(A,B,dt1); 


SiFilter 
i21; 
Е=.01; 
0-0; 
А=[0 1 0:0 0 1:0 0 0); 
B=[O O 1]'; 
Н=[1 0 0]; 
[Phi,De1l]=c2d(A,B,dt1); 
XKKM1=zeros(3,1); 
РККМ1=еуе (3) *1е7; 
Phi IR-eye(3); 
count=0; 
for K=1:kmax 
G(:,k)-PKKM1*H'*(H*PKKM1*H'-R)^(-1); 
PKK-(eye(3)-G(:,k) *H) *PKKM1; 
PKKM1-Phi*PKK*Phi'-4Q; 
XKK(:,kK)=XKKM1(:,k)+G(:,k) *(theta(k) -XKKM1(1,k)); 
if count*100+1 == 
G(:,1)=PKKM1*H'* (H* PKKM1*H'+R)%*(-1) ; 
PKK-(eye(3)-G(:,i)*H)*PKKM1; 
PKKM1=Phi_IR*PKK*Phi_IR'+Q; 
XKKM1(:,1)=XKK(:,k); 
XKK(:,1)-XKKM1(:,i)4G(:,i)*(theta(k)-XKKM1(1,1)); 
XKKM1(:,k-*1)-2Phi*XKK(:,i); 
count=count+1; 
ісі-1; 
else 
XKKM1(:,k-*1)-2Phi*XKK(:,k); 
end 
x(:,Kk+1)=phixwx(:,KXK); 
theta(k+1)=atan2(x(1,Kk+1),x(3,K+1)); 
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end 
subplot(221) 
plot(x(1,:),x(3,:)),grid,ylabel('(Y in (Km))') 
title('Missiles track'),xlabel('(X in (Km))') 
plot(theta*180/pi),grid,title('Bearing to the missile') 
xlabel('Samples taken (t-.01)'),ylabel('Degrees') 
plot(XKK(1,:)*180/pi),grid 
title('Estimate (For Radar) ') 
xlabel('Samples taken (t-.01)'),ylabel('Degrees') 
for k=1:500 

t (k) =dt1*k; 
end | 
plot(t,G(1,1:500)),title('G(1,: 
DOC (t,G(2,1:500) ), title('G(2,: 
mot(t,G(3,1:500)),title('G(3,:)"') 
plot(XKK(2,:)),title('Angular velocity!) 
plot(XKK(3,:)),title('Angular acceleration!) 


)'),pause 
)') 
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F. 8 ELLIPSE.M 


This program will produce error ellipsoids for the first 
few samples of the track. 


echo off 
!del pic3.met 
load c:\matlab\wiseman\output2.dat 
load c:\matlab\wiseman\truepos2.dat 
load c:\matlab\wiseman\pkkout.dat 
r=9; 
axis('square') 
axis([-50 500 -50 500]) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%355%5%% 
€ plot path and estimate of path % 
%%%5%%%%%%%%%%%%%%%%%%%%%5%%5%5%5%5%%5%%5%5%%%5%%9%% 
plot(output2(1:r,1),SüutpóüucC2(1 r 2) 0 
truepos2(1:r,2),truepos2(1:r,3)) 

xlabel('X - Position (ft)');ylabel('Y - Position (ft)') 
title('Trajectory - S Curve") 
gtext(' True track!) 
gtext('+ + + Estimate!) 
gtext('O O Error Ellipsoids') 
gtext('Enlarged 10 times') 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%%%%5%%%%%%%%5%%%%%%%%2% 
% generate a uniformly spaced set of points 
%%%%%%%%%%%%%%%%%%%%5%%%%%%%%%5%%%%%5%%%%%%%%%%%%%%9%% 
delt=-1:.05:1; 
delt-pi*delt; 
%%%%%%%%%%%%%%%%%%%5%%2%2%%%%%%%%%5%%%%%5%%%%%%%%%9%% 
% generate a (2xm) set of vectors on the unit c 
%%%%%%%%%%%%%%%%%%%%%%%%%%5%52%2%%%%%%%%%%%%%%%%5%% 
y=[cos(delt);sin(delt)]; 
shift=diag(ones(41)); % 41 points in delt 
shift=shift'; 
hold on 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%%%%5%9%5%5%% 
$ The function chol, to compute the Choleski % 
% decomposition, is done "backwards" in MATLAB. It % 
$ makes an upper triangular matrix R, not L. So take % 
$ the transpose. % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
for k=1:9 

i-k-1; 

P=pkkout (2*1+1:2*1+2,1:2); 

s-chol(P)'; 

X=S*y; 

plot(x(1,:)*10+output2(k,1)*shift, 

x(2,:)*10+o0utput2 (k,2) *shift) 





oo o? o? 


end 
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%%%%5%%%%%5%%%%%%%5%5%%%%%%%%5%5%%%%5%%%%%5%5%%%%%%%5%%5%%5%5%5%%55%% 
% An array of x-vectors (2xm) has been created that % 
% form the error ellipsoid. $ 
%%%343343$%$%$%354/0  St3333⁄4393Fç4%$%$%%$33q9 93 3%$%*%%3%%35 340.|q513%3%$3433%333534 y 51]%%$%%%%%% 
hold off 

pause 

meta pic3 
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G. TORPSCUR.M 


This is the program to generate a true track and a monte 
carlo track. The program generates a simulation for a 40 knot 
torpedo following an s-shaped track. 

Velocity is in yards/second and position is in feet. Data 
orientation is similar to actual data. 

ndim = 3; 
% 
% 
%%%%% Establish Measurement Standard Deviations %%%%%%%%% 
% 


sigmapos = diag([ 15.0 , 15.0 , 0.00 ]) ; 

sigmavel = diag([ 0.01 , 0.01 , 0.00 ]) ; 

sigmaacc - diag([ 0.03 , 0.03 , 0.00 ]) ; 
t 
$$ Let User Input Number of "Noisy" Tracks to Generate %% 
%% and the Seed for the Random Number Generator %% 
% 

nsimul = input(['Enter the Base Reference Number for 

this Simulation (100 <= n <= 900): ']) ; 
nsimul = fix(min(max(nsimul,100) ,900) ) ; 
fprintf(' Your Base Reference Number is - %4.0f\n', 
nsimul) ; 

% 

пегаск = input('Enter Number of Random Tracks to 

Generate: ') ; 
nseed = 0 ; 
if пігаск > 0 ; - І 
пѕееа = input('Enter Seed for Random Number 
Generator: '); 

end 
% 
%%5%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%5%%%%%%%%%%%%%%5%%%%%%%%%%%%% 
%%%% Тһе Following Section Generates the "True" %%%%% 
%%%% and Simulated Tracks. %%%%% 
%%%% This Sectlon Must Be Changed as Appropriate %%%%% 
%%%% in Order to Generate Other Tracks %%%%% 


%%%%%%%%%%%%5%%%%%5%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%%%%%% 
%%%%%%%%% 

%%%%%%%%%%%%% Establish Model Constants %5%%%%%%%%%%%%% 
% 


r = 250; ШЕ 
deltat = 0.5; 

tfinal = 45.0; 

Кпах = tfinal/deltat + 1; 

ydconv = 3.0; ` 
v40kt = 200/9 ; к 
thetadot - ydconv*v4Okt/r ; il 
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% 


%%%5%%%%%%%%%% Initialize Data Arrays %%%%%%%%%%%%%%% 
% 

x = zeros(kmax,ndim); 

у = zeros(kmax,ndim); 

a = zeros(kmax,ndim); 
% 
%%%%% Generate Accelerations for a S-Track %%%% 
% 

ШЕ 1:20 - зо зо т; 

theta = thetadot*deltat*( (0:119 )'+0.5) ; 

а(25:34,1) = -12.%опе5(10,1); 

а(45:54,2) — -12.*ones(10,1); 

a(65:74,1:2) = опез(10,1)%( 12. 12. ); 
% 
% 
%%%%% Then Generate Velocities and Positions by %%%%% 
%%%%% Piecewise Constant Integration %%%%% 
% 
% 

Ғог і-2:Кпах ; 

(1,2) = v(i-1,:) + a(1-1,:)*deltat/ydconv 2 
ЖІ, ?) = х(1-1,:) * ydconv*v(i-1,:)*deltat 2 
* a(i-1,:)*deltat^2/2 ; 

end 
% 
%%%%%%5%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5%%%%%%5%%%% 


% 
%%%%% 
% 


Plot the "True" Track 

eval( [ '! del dataNtrack',num2str(nsimul),'.met' 
plot(x(1:kmax,1),x(1:kmax,2));xlabel('x-axis'); 
ylabel('y-axis');grid; 


]) 


eval( [ ‘meta data\track',num2str(nsimul) }) 
% 
% and Save the True Track Position/Velocity/Acceleration %% 
$ 
eval( [ 'save dataNdelt',num2str(nsimul),' deltat ' )) 
eval( [ 'save dataNtruep',num2str(nsimul),' x ' ]) 
eval( [ 'save dataNtruev',num2str(nsimul),' У ' }) 
eval( [ 'save dataNtruea',num2str(nsimul),' а ' ]) 
% 
%%%%% Next Generate the Requested Number of "Noisy" %%%%% 
%%%%% Tracks by Adding Random Noise with the %%%%% 
%%%%% Previously Specified Standard Deviations to %%%%% 
%%%%% the "True" Track. (This is done is a loop, %%%%% 
%%%%% with each track graphed and saved as soon as %%%%% 
%%%%% it is generated.) %%%%% 
% 


truex x ; truev truea 


rand('normal'); 


у; a; 
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if nc rack > 07; 
rand('seed',nseed); 


eval( [ ‘save data\seed',num2str(nsimul),' nseed ' }) 
for j-1:ntrack ; $ Generate Tracks 
x = truex + rand(kmax,3)*sigmapos ; 
v — truev + rand(kmax,3)*sigmavel ; 
а = truea + rand(kmax,3)*sigmaacc ; 
$ $ Then Save them, using 
t $ the 'eval' function 
eval( [ 'save dataNtstp',num2str(nsimul-j),' x ' ]) 
eval( [ 'save dataNtstv',num2str(nsimul4j),' v ' J) 
eval( [ 'save dataNtsta',num2str(nsimul*j),' a ' ]) 
% 
с1а ; $ Then Plot Them 
eval( [ '! del dataNtrack',num2str(nsimul-j),'.met' ]) 


plot(truex(1:kmax,1),truex(1:kmax,2),x(1:kmax,1),x(1:kmax,2) 
,'--'),title('x (-) and xrandom (--)') ; 
xlabel('x-axis'),ylabel('y-axis'),grid 
eval( ( 'meta dataWNtrack',num2str(nsimul-j) ]) 
end ; end 
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H.  TRACKSIM.M 
% 
% TRACKSIM.M 
% THIS PROGRAM GENERATES SIMULATED TORPEDO TRACKS FOR 
% TESTING THE KEYPORT KALMAN FILTERING. THIS PROGRAM AS 
% CURRENTLY WRITTEN GENERATES A SIMULATION FOR A 40 KNOT 
% TORPEDO ENTERING A 250 FT RADIUS CIRCLE AT A TURNING RATE 
% ОҒ 15 DEGREES/SECOND. THE VELOCITY IS IN YARDS/SECOND 
% AND POSITION IS IN FEET, AND THE DATA ORIENTATION IS 
% SIMILAR TO ACTUAL DATA. HOWEVER, THE PROGRAM IS DESIGNED 
% SO THAT ONE SIMPLY NEED CHANGE THE ACCELERATION DATA (IN 
% THE PORTION OF THE CODE WHERE INDICATED) 
% 
% 

ndim = 3; 
% 
% 
%%%%%% Establish Measurement Standard Deviations %%%%%%%%% 
% 

Sigmapos = diag({ 15.0, 15.0, 0.00 ]) ; 

Sigmavel = diag({ 0.01 , 0.01 , 0.00 ]) ; 

Sigmaacc = diag([ 0.03 , 0.03 , 0.00 }) ; 
% 
%%% Let User Input Number of "Noisy" Tracks to Generate %%% 
%%% -~ and the Seed for the Random Number Generator %%% 
% 

nsimul = input(['Enter the Base Reference Number for 

this Simulation (100 <= п <= 900): ']) ; 

nsimul —- fix(min(max(nsimul1,100),900) ) ; 

fprintf('Your Base Reference Number is - $4.0fMn',nsimul); 
$ 

ntrack = input('Enter Number of Random Tracks to 

Generate: ') ; 
nseed = 0 ; 
if ntrack » O ; 
nseed = input('Enter Seed for Random Number 
Generator: '); 

end 
% 
%%%%%%%%%%%%%%%%%%%5%%%%%%%%%%%%%%%%%5%%%%%%%%%5%%%%%%%%%%%9%53% 
%%% Тһе Following Section Generates the "True!" %%% 
%%% and Simulated Tracks. %%% 
%%% This Section Must Be Changed as Appropriate %%% 
%%% in Order to Generate Other Tracks itt 


%%%%%%%%%%%%%%%%%%%%5%%%%%5%5%5%5%5%%%5%5%%5%5%%%5%%%5%%5%5%%%%%5%%5%%%5%%5%%% 
% 
%%%%%%%%%%%%%% Establish Model Constants %%%%%%%%%%%%%%% 


% 
r = 250; 
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deltat = 05; 
tfinal = 80.0; 
kmax = tfinal/deltat + 1; 
ydconv = 3.0; 
v4Okt = 200/9 ; 
thetadot - ydconv*v4Okt/r ; 
$%%%%%%%%%%%%% Imitialize Data Arrays %%%%%%%%%%%%%% 
% 
x = Zeros(kmax,ndim); 
V = zeros(kmax,ndinm) ; 
a = zeros (kmax,ndim) ; 
% 
% 
%%%% Generate Accelerations for a Circular Track %%%%% 
% 
v(1,:) = [ vç4Ço kt 0 On; 
theta = thetadot*deltat*( (0:119 )'+0.5) ; 
a(20,1:2) = [ -v40kt/fdeltat , v40kt/deltat | 
*ydconv ; 
a(21:140,1:2) » ( (v40kt*ydconv)^2/r ) 
*[ -cos(theta) , -sin(theta) ] ; 
% 
% 
%%%%% Then Generate Velocities and Positions by  %%%%%%%%% 
%%%%% Piecewise Constant Integration %%%%%%%%% 
% 
% 
Ғог і-2:Кпах ; 
VOI) = v(i-1,:) -* a(i-1,:)*deltat/ydconv ; 
XC) = x(i-1,:) + ydconv*v(i-1,:)*deltat ... ; 
-* a(i-1,:)*deltat^2/2 ; 
end ; 
% 
%%5%%%%%%%%%%%%%%%%%%%%%%%%%5%%%%%%%%%%%%%5%5%%%%%%%%%%%%%%5%%%%% 
% 
%%%%%% Plot the "True" Track %%%%%%%%%%% 
% 
еуа1( [ '! del dataNtrack',num2str(nsimul),'.met' )) 
plot(x(1:kmax,1),x(1:kmax,2));xlabel('x-axis'); 
ylabel('y-axis');grid; 
eval( [ 'meta data\track',num2str(nsimul) }) 
t 


$ and Save the True Track Position/Velocity/Acceleration %% 
% 


eval( [ 'save data\delt',num2str(nsimul),' deltat ' |) 
eval( [ 'save dataNtruep',num2str(nsimul),' x ' ]) 
eval( [ 'save dataNtruev',num2str(nsimul),' v ' ]) 
eval( [ 'save data\truea',num2str(nsimul),' a ' }) 

% 

%%%%% Next Generate the Requested Number of "Noisy" %%%%% 

%%%%% Тгаскв Бу Adding Random Noise with the %%%%% 
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%%%%% Previously Specified Standard Deviations to %%%%% 


$3131 the "True" Track. (This is done is a loop, $3331 
%%%%% wlth each track graphed and saved as soon as %%5%%% 
%%%%% it is generated.) %%%%% 
% 


truex = x ; truev = v ; truea » a ; 
rand('normal'); 


% 
if ntrack » O ; 
rand('seed',nseed); 
eval( [ 'save data\seed',num2str(nsimul),' nseed ' J) 
for j=l:ntrack ; $ Generate Tracks 
x = truex + rand(kmax,3)*sigmapos ; 
v = truev + rand(kmax,3)*sigmavel ; 
а = truea + rand(kmax,3)*Sigmaacc ; 
% + Then Save 
them, using 
% $ the 'eva]l' 
function 
eval( [ ‘save data\tstp',num2str(nsimul+j),' x ' J) 
eval( [ 'save dataNtstv',num2str(nsimul-4j),' v ' ]) 
eval( [ 'save data\tsta',num2str(nsimul+j),' а ' j) 
* 
erg; % Then Plot Them 


eval( ( '! del dataNtrack',num2str(nsimul-4j),'.met' ]) 


plot (truex(1:kmax,1) ,truex(1:kmax,2),x(1:kmax,1) ,x(1: kmax, 2) 


gp ——— e 
, 


title('x (-) and xrandom (--)') ; 
xlabel('x-axis'),ylabel('y-axis'),grid 


eval( [ 'meta dataNtrack',num2str(nsimul-*j) ]) 
end ; end 


49 


APPENDIX C: THE KALMAN FILTER AND MANEUVER DETECTION 


A. OBSDATA. FOR 


С OBSDATA.FOR 

C MODIFICATIONS MADE BY ART SCHOENSTADT ON AUGUST 14, 1992. 
C 

C---THIS PROGRAM IS DESIGNED TO PROCESS 3/D DATA FILES WHICH 
C---SIMULATE THE UNDERSEA TRACKING RANGE AT KEYPORT, WA. 
C---THE INPUT DATA FILES ARE GENERATED BY A MATLAB SIMULATION. 
C---THIS PROGRAM IS DESIGNED TO RUN ON THE IBM/AT PERSONAL 
C---COMPUTER. THE USER IS ALSO REQUIRED TO PROVIDE THE INPUT 
C---DATA FILES IN SPECIFIC FORMAT, AND THE NUMBER OF POINTS TO 
C---CALCULATE (MAXIMUM OF 2500). THE PROGRAM USES A 
C---LARGE AMOUNT OF HARD DISK SPACE FOR TEMPORARY 

C---FILES (APPROXIMATELY 160K BYTES/100 POINTS, 4MEG BYTES MAX 
C---FOR 2500 POINTS. 

C---THE PROGRAM ALSO INCLUDES "HARD WIRED" CONVERSIONS TO 
C---REFLECT THE FACT THAT SOME DATA IS IN FEET, OTHER DATA IN 
C---YARDS, AND DEPTH IS MEASURED IN POSITIVE, NOT NEGATIVE 
C=== TERMS. 

C 

C * * k k k k k k k k k k k k * * SECTION 1 k k k k k k k k k k k k k k k k k k k k k k k k k k 
C 

C*** DECLARATION OF PARAMETERS *** ` 

C*** MAXOBS IS THE MAXIMUM NUMBER OF OBSERVATIONS THAT ЖЖЖ 


C*** CAN BE USED ххх 
C*** MAXSTATE IS SET FOR A 3-DIMENSIONAL, POSITION, kk 
СЖ ж VELOCITY AND ACCELERATION PLANT kkk 
C*** DEPS IS USED TO TEST WHEN TWO OBSERVATIONS ARE CLOSE *** 
C*** ENOUGH TO BE CONSIDERED SIMULTANEOUS kkk 
С 


PARAMETER( MAXOBS=250, MAXSTATE=9 ) 
PARAMETER( DEPS-0.005D0, STEP-0.5D0 ) 


***** DECLARATION OF VARIABLES ***** 


ооо 


DOUBLE PRECISION PTC(MAXOBS),VTIME(MAXOBS), 

E SOURCE (MAXOBS) , ZK(MAXSTATE , MAXOBS) , ROLL(MAXOBS) 
DOUBLE PRECISION SS,SSV,SEC,X,Y,Z2,XVEL, 

* — YVEL,ZVEL, ACCX, ACCY, ACCZ, EYAW, EROLL, EPITCH, PTM, VTM, 
* ATM, RTM, ATM1, ACCXM1, ACCYM1, ACCZM1,RLLM1, SSA, SSR, 

* TORANGE, TOTORP, TNEXTOBS 


INTEGER HH, HHV,MM,MMV,HOUR,MIN, PC, MMA, MMR, NOBSERV, SRC 
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Q0000 


оо о 


Оооо 


(0000000 


CHARACTER ASK*1 


CHARACTER PKKS3D*13, PKKM1S*13, INFILC*13, INFILR*13 
CHARACTER INFILP*13, INFILA*13, OUTFIL*13 
LOGICAL*1  POSREC,VELREC,ACCREC,ENDPOSDAT, ENDVELDAT, 
* ENDACCDAT , ENDRLLDAT 
**** INPUT DATA, DESIGNATE FILENAMES *xx* 
OPEN(1, FILE= 'DATA\POSITION.OBS', STATUS= 'OLD') 
ОРЕМ(2, FILE= 'DATANVELOCITY.OBS', STATUS- 'OLD') 
OPEN(3, FILE- 'DATANMACCELERA.OBS', STATUS- 'OLD') 
OPEN(4, FILE= 'DATANROLL YAW.OBS', STATUS- 'OLD') 
OPEN(11,FILE= 'DATA\OBSERVAT.DAT!, STATUS= 'NEW') 
**** NOTIFY USER OF MAXIMUM NUMBER OF DATA POINTS **x* 
WRITE(*,770) МАХОВ5 
770 FORMAT(' The MAXIMUM number of points that can be ', 
* 'analyzed is:',I4) 
WRITE(*,771) 
771 FORMAT(' To use more, you must reset the parameter ', 
* 'MAXOBS',/,' in the main program and recompile') 
K k k k k k k k k k k k k k k k SECTION 2 K k k k k k k k k k k k k k k k k k k k k k k k k k 
**** GET STARTING TIME FOR RANGE DATA **** 
READ(1,750,END-1200) PC,X,Y,2,HH,MM,SS 
WRITE(*,751) HH,MM,SS 
TORANGE = 3600. *REAL(HH) +60. *REAL(MM)+SS 
WRITE (*, 752) 
READ (*,755) HOUR,ASK,MIN,SEC 
IF (ASK .EQ. ' ') GOTO 754 
TORANGE = 3600.*REAL(HOUR) +60. *REAL(MIN) +SEC 
WRITE(*,756) HOUR,MIN,SEC 
754 CONTINUE 


750 FORMAT(1X,16,2X,3F10.1,10X,12,1X,I2,1X,F5.2) 

751 FORMAT(/,' The first position time 
ДЕ на 12. '<+:  Е27.4) 

752 FORMAT(/,' If you do not wish to change it, press 
*"Enter"',/,' If you wish to change it, enter the new 
*time in the format: ',/,' HH:MM:SS.SSSS') 

755 FORMAT(I2,A1,12,1X,F7.4) 

756 FORMAT(' The first position time is 
ХОТЕЛ 12. = ' Е7.4) 


51 


ооо 


(300000000 


**** GET STARTING TIME FOR TORPEDO DATA **x*x 


READ(2,760,END-1201) MMV,SSV,XVEL, YVEL, ZVEL 
HHV = 0 

TOTORP = 60.*REAL(MMV)+SSV 

WRITE(*,761) HHV,MMV,SSV 


WRITE(*,752) 
READ(*,755,END=763) HOUR,ASK,MIN,SEC 
IF (ASK .EQ. ' ') GOTO 763 


TOTORP = 3600.*REAL(HOUR)+60.*REAL(MIN)+SEC 
WRITE(*,762) HOUR,MIN,SEC 
763 CONTINUE 


760 FORMAT(1X,I2,1X,F7.4,10X,F9.3,5X,F9.3,5X,F9.3) 
761 FORMAT(/,' The first velocity time 


kis:',I2,':',I2,':',F7.4) 
762 FORMAT(' The first velocity time is 
*nows ' 12.03 © 12 ЕЛ И 


ЖЖЖЖ SELECT THE FIRST RECORD FROM EACH DATA SET ТНАТ *%*%Ж% 
**** REPRESENTS AN OBSERVATION AT OR AFTER THE STARTING ****x 
**** TIME. NOTE THAT WE MUST USE A SLIGHTLY DIFFERENT ЖЖЖЖ 
**** PROCEDURE FOR THE POSITION AND VELOCITY DATA ЖЖЖЖ 
**** FILES, SINCE WE HAVE ALREADY READ A RECORD FROM KKK 
**** EACH OF THEM. ЖЖЖЖ 


201 CONTINUE 
IF ( PTM .GE. TORANGE ) GOTO 202 
READ(1,750,END=1200) PC,X,Y,Z,HH,MM,SS 
PTM = 3600.*REAL(HH)+60.*REAL(MM) +SS 
GOTO 201 


202 CONTINUE 
IF ( VTM .GE. TOTORP ) GOTO 203 
READ(2,760,END=1201) MMV,SSV,XVEL, YVEL, ZVEL 
VTM = 60.*REAL(MMV) +SSV 
GOTO 202 


203 CONTINUE 
READ (3,760,END=1202) MMA,SSA,ACCX,ACCY,ACCZ 
ATM = 60.*REAL(MMA)+SSA 
IF ( ATM .LT. TOTORP ) GOTO 203 
ATM1=ATM 
ACCXM1=ACCX 
ACCYM1=ACCY 
ACCZM1=ACCZ 


204 CONTINUE 
READ (4,760,END=1203) MMR,SSR,EYAW, EROLL, EPITCH 
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RTM = 60.*REAL(MMR)+SSR 

IF ( RTM .LT. TOTORP ) GOTO 204 
RTM1=RTM 
RLLM1=EROLL 


C 
С n 
e *k*kk SYNCHRONIZE RANGE AND TORPEDO CLOCKS  *****kx 
С 3 
PTM = РТМ - ТОКАМСЕ 
VTM = VTM - TOTORP 
ATM - АТМ - ТОТОКР 
КІМ = RTM - TOTORP 
С 
C * k k k k k k k k k k k k k k k SECTION 3 k k k k k k k k k k k k k k k k k k k k k k k k k 
C 
C*** BUILD THE OBSERVATION ARRAY BY SEQUENTIALLY kkk 
C*** DETERMINING THE NEXT OBSERVATION TIME AND THE ЖЖЖ 
C*** SPECIFIC FIELDS OBSERVED AT THAT TIME kkk 
C 
С 
WRITE(*,*) "' ' 
WRITE(*,*)  'Reading Data Files' 
NOBSERV = O 
ENDPOSDAT = .FALSE. 
ENDVELDAT = .FALSE. 
ENDACCDAT = .FALSE. 
ENDRLLDAT = .FALSE. 
С 
300 СОМТІМСЕ 
IF (ENDVELDAT .AND. ENDPOSDAT ) GOTO 400 
IF (NOBSERV.GE.MAXOBS) GOTO 395 
С 
NOBSERV = NOBSERV + 1 
POSREC = .FALSE. 
VELREC = .FALSE. 
ACCREC = .FALSE. 
TNEXTOBS = DMIN1(PTM,VTM) 
IF (ENDPOSDAT) TNEXTOBS = VTM 
IF (ENDVELDAT) TNEXTOBS = PTM 
VTIME(NOBSERV) = TNEXTOBS 
С 
C*** SEE IF THERE'S A POSITION OBSERVATION AT THIS TIME *** 
C 


311 CONTINUE 


IF ( ( (PTM-TNEXTOBS) .LT. DEPS ) 


( .NOT. (ENDPOSDAT) ) ) THEN 


POSREC = .TRUE. 
PTC (NOBSERV) =PC 
ZK(1,NOBSERV) =X 
ZK(4,NOBSERV) =Y 
ZK(7,NOBSERV) =Z 
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. AND. 


READ(1,750,END-312) PC,X,Y,Z,HH,MM,SS 
PTM - 3600.*REAL(HH)-460.*REAL(MM)«*SS - TORANGE 
IF ( PTM .LT. TNEXTOBS ) GOTO 1210 


ENDIF 
GOTO 321 
С 
312 ENDPOSDAT-.TRUE. 
WRITE(*,313) NOBSERV 
313 FORMAT(' End of Position Record File Encountered at ', 
* 'Observation ',I4) 
C 
C 


Crk SEE IF THERE'S A VELOCITY OBSERVATION AT THIS TIME *** 


С 
321 CONTINUE 

IF ( ( (VTM-TNEXTOBS) .LT. DEPS ) .AND. 

* ( .NOT. (ENDVELDAT) ) ) THEN 
VELREC = .TRUE. 
ZK(2,NOBSERV)= 3.*XVEL 
ZK(5,NOBSERV) =-3.*YVEL 
ZK(8,NOBSERV) =-3.*ZVEL 
READ (2,760,END=322) MMV,SSV,XVEL, YVEL, ZVEL 
VIM = 60.*REAL(MMV)+SSV - TOTORP 
IF ( VIM .LT. TNEXTOBS ) GOTO 1211 


ENDIF 
GOTO 331 
С 
322 ENDVELDAT=.TRUE. 
WRITE(*,323) NOBSERV 
323 E натр End of Velocity Record File Encountered at ', 
'Observation ',I4) 
C 
C 


C* SEE IF THERE'S AN ACCELERATION OBSERVATION AT THIS TIME % 
C* THEN USE THE CLOSEST ACCELERATION OBSERVATION TO THE 
C* CURRENT TIME. k 
С 
331 CONTINUE 
IF ( ( ATM .LE. TNEXTOBS ) .AND. 
( .NOT. (ENDACCDAT) ) ) THEN 
ATM1-ATM 
ACCXM1-ACCX 
ACCYM1=ACCY 
ACCZM1=ACCZ 
332 READ (3,760, END=333) MMA,SSA,ACCX,ACCY,ACCZ 
ATM = 60.*REAL(MMA)+SSA - TOTORP 
IF ( ATM .LE. TNEXTOBS ) GOTO 332 
ENDIF 
GOTO 335 


* 


С 
333 ENDACCDAT-.TRUE. 
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С 


WRITE(*,334) NOBSERV 
334 FORMAT(' End of Acceleration Record File Encountered at 
*Observation ',I4) 


335 CONTINUE 

IF ( (TNEXTOBS-ATM1) .LT. (ATM - TNEXTOBS) ) THEN 
ZK(3,NOBSERV) -ACCXM1 
ZK(6,NOBSERV) -ACCYM1 
ZK(9,NOBSERV) -ACCZM1 

ELSE 

ZK(3,NOBSERV) -ACCX 
ZK(6,NOBSERV) -ACCY 
ZK(9,NOBSERV) -ACCZ 

ENDIF 


C*** SEE IF THERE'S A ROLL OBSERVATION AT THIS TIME kkk kk 
C*** THEN USE THE CLOSEST ROLL OBSERVATION TO THE K k k k * 
Сххх CURRENT TIME. k k k k + 


С 
= 


341 CONTINUE 
IF ( ( RTM .LE. TNEXTOBS ) .AND. 
* ( .NOT.(ENDRLLDAT) ) ) THEN 
RTM1=RTM 
RLLM1=EROLL 
342 READ(4,760,END=343) MMR,SSR,EYAW,EROLL,EPITCH 
RTM = 60.*REAL(MMR)+SSR - TOTORP 
IF ( RTM .LT. TNEXTOBS ) GOTO 342 
ENDIF ~ 
GOTO 345 
343 ENDRLLDAT=.TRUE. 
WRITE (*,344) NOBSERV 
344 FORMAT(' End of Roll Record File Encountered at ', 


* 'Observation ',I4) 
c 
345 CONTINUE 
IF ( (TNEXTOBS-RTM1) .LT. (RTM - TNEXTOBS) ) THEN 
ROLL(NOBSERV) =RLLM1 
ELSE 
ROLL (NOBSERV) =EROLL 
ENDIF 
ACCREC = .TRUE. 
C 
C**** CODE THE SOURCE(S) OF THIS RECORD k k k k * * 
C 
351 CONTINUE 
SRC = 0 
IF ( POSREC ) SRC = 73 И 
IF ( VELREC ) SRC = SRC + 146 a 
С IF ( ACCREC ) SRC = SRC + 292 


SOURCE(NOBSERV)= SRC 
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C 


С 
GOTO 300 
С 

395 WRITE(*,396) NOBSERV,MAXOBS 

396 FORMAT(' The total number of observations (',I4,') 
*exceeds allocate storage (MAXOBS = ',14,/,5x,'Some data 
*will be lost. You may wish to recompile the program',/, 
* with a higher value of MAXOBS. (Enter "Y" to 
*continue) ') 

READ(*,397) ASK 
397 FORMAT(A1) 
IF ( ( ASK .NE. 'Y' ) .AND. ( ASK .NE. 'y' ) ) STOP 
C 
C 
C k * k k k k k k k k k k k k k k k SECTION 4 * * k k k k k * k k k k k k k k k k k k k k k k kx 
C**** WRITE OUT DATA FILES ЖЖЖЖ 
С 
e 
400 CONTINUE 
CLOSE(1) 
CLOSE(2) 
CLOSE(3) 
CLOSE (4) 
WRITE(*,902) NOBSERV 
WRITE(11,901) NOBSERV 
DO 401 IPRT = 1,NOBSERV 
WRITE(11,900) PTC(IPRT),VTIME(IPRT),SOURCE(IPRT), 
k ( ZK(JPRT,IPRT),JPRT-1,MAXSTATE) , ROLL(IPRT) 

401 CONTINUE 

900 FORMAT(13(1X,E14.8)) 

902 FORMAT(/,' Writing ',I5,' Combined Observations to 
*File') 

901 FORMAT(' OUTPUT FROM PROGRAM OBSDATA.FOR ',/, 
*1X,14,' OBSERVATIONS’, 
*/,7X,'PTC',11X, 'TIME',10X, 'SOURCE',10X, 'ZK(1)',10x, 
*'ZK(2) ', 10X, 'ZK(3) !, 10X, 'ZK(4) ', 10X, 'ZK(5) ' , 10X, 'ZK(6) !, 
*10X,'ZK(7)',10X, 'ZK(8) ', 10X,'ZK(9) ', 10X, ' ROLL!) 

CLOSE(11) 
C 
C 
STOP 
Ç 
C **** ERROR ROUTINES FOR UNEXPECTED END OF DATA FILE **** 
С 

1200 WRITE(*,1300) 

1300 FORMAT('!**** ERROR - RANGE DATA FILE ENDED BEFORE A 
*VALID ','RECORD COULD BE LOCATED!) 

STOP 
C 


1201 WRITE(*,1301) 
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1301 FORMAT('**** ERROR - VELOCITY DATA FILE ENDED BEFORE A 
*VALID RECORD COULD BE LOCATED') 
STOP 
С 
1202 WRITE(*,1302) 
1302 FORMAT('**** ERROR - ACCELERATION DATA FILE ENDED BEFORE 
*A VALID RECORD COULD BE LOCATED!) 
STOP 
C 
1203 WRITE(*,1303) 
1303 FORMAT('**** ERROR - ROLL DATA FILE ENDED BEFORE A VALID 
*RECORD COULD BE LOCATED!) 
STOP 
C 
C 
1210 WRITE(*,1310) 
1310 FORMAT('**** ERROR - RANGE DATA FILE RECORD OUT OF VALID 
*TIME ORDER!) 
STOP 
C 
1211 WRITE(*,1311) 
1311 FORMAT('**** ERROR - VELOCITY DATA FILE OUT OF VALID 
*TIME ORDER!) 
STOP 
S 
1212 WRITE(*,1312) 
1312 FORMAT('**** ERROR - ACCELERATION DATA FILE OUT OF VALID 
*TIME ORDER!) 
STOP 
С 
1213 WRITE(*,1313) 
1313 FORMAT('**** ERROR - ROLL DATA FILE OUT OF VALID 
*TIME ORDER!) 
STOP 


END 
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000000000 


С) С) GONO O O С) С) С) С) O O 


С) С) 


ООООООО о о 


POSCONV. FOR 


POSCONV. FOR 


THIS PROGRAM CONVERTS THE DATA FILES CREATED BY THE 
MATLAB SIMULATION FOR POSITION, VELOCITY, AND 
ACCELERATION INTO THE PROPER FORMAT FOR USE BY THE KALMAN 
FILTER PROGRAM. IT ALSO CREATES A ROLL ANGLE DATA FILE 
OF O DEGREES THROUGHOUT. 


INTEGER PC,MIN,HR 

DOUBLE PRECISION SEC,X,Y,2,VX,VY,VZ,AX,AY,AZ,YAW, 
DOUBLE TRECISION ROLL, PITCH, TOTSEC, DELTAT 
LOGICAL*1 ENDPOSREC, ENDVELREC, ENDACCREC 


k****** OPEN THE TIMING INFORMATION DATA FILE *********** 
OPEN (1,F ILE='DATA\DELTSIMU', STATUS='OLD') 


k****** OPEN THE INPUT POSITION DATA FILE kkkkkkkkkkk 
OPEN (2, FILE='DATA\SIMULPOS'! , STATUS='OLD') 


k***** OPEN THE INPUT VELOCITY DATA FILE kk kk k k k k * k k 
OPEN (3, FILE='DATA\SIMULVEL! , STATUS='OLD') 


****** OPEN THE INPUT ACCELERATION DATA FILE *kkkkkkkkkk 
OPEN (4, FILE='DATA\SIMULACC! ,STATUS='OLD') 
kkkkkk OPEN THE OUTPUT POSITION DATA FILE kk kk RRR RR KE 
OPEN (12, FILE='DATA\POSITION. OBS! ,STATUS='NEW') 
****** OPEN THE OUTPUT VELOCITY DATA FILE kkkkkkkkkk* 
OPEN (13 ,FILE='DATA\VELOCITY.OBS' , STATUS='NEW') 
kkkkkk OPEN THE OUTPUT ACCELERATION DATA FILE **kkkkkkkkk 
OPEN(14,FILE-'DATANACCELERA.OBS',STATUS-'NEW!) 
****** OPEN THE OUTPUT ROLL ANGLE DATA FILE kkk kkk kkk kk 
OPEN(15,FILE-'DATANROLL YAW.OBS',STATUS-'NEW!) 
* k k k k k k k k k k k k k k k k k k k k k k k k k k k k k kk k k eoe eoe dee k k kk k k k k k k k k k k k k k k k 
kkk INPUT THE MATLAB FILES AND CONVERT TO INPUT ххх 
к FORMAT FOR THE FILTER PROGRAM dk 


* k k k k k k k k k k kk k kk k k k k k kk kk k k k k kk kk kk k k k k kk k k k k k k k k k k k k k k k k kx x 


PC=1 
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O00 ООО O O O 


ООО 


O00 


ООО 


100 


ххх 


ххх 


ххх 


ххх 


ххх 


ххх 


TOTSEC- 0.DO 
READ(1,20) DELTAT 


ENDPOSREC = .FALSE. 
ENDVELREC = .FALSE. 
ENDACCREC = .FALSE. 
CONTINUE 


TF 


( ENDPOSREC .AND. ENDVELREC .AND. ENDACCREC ) 
GOTO 200 


UPDATE TIMING DATA 


SEC = AMOD(TOTSEC, 60.0) 
MIN - MOD( DINT(TOTSEC/60.D0), 60 ) 
HR - DINT(TOTSEC/3600.D0) 

PROCESS ROLL, YAW AND PITCH DATA 
YAW-.00 
ROLL-.00 
PITCH-.00 


WRITE(15,40) MIN,SEC,YAW,ROLL, PITCH 


PROCESS POSITION DATA 
IF ( .NOT. ENDPOSREC ) THEN 
READ(2,20,END-101) X,Y,Z 
WRITE(12,30) PC,X,Y,Z,HR,MIN, SEC 
ENDIF 


PROCESS VELOCITY DATA 


IF ( .NOT. ENDVELREC ) THEN 
READ(3,20,END-102) VX,VY,VZ 


VY=-VY 

VZ--VZ 

WRITE(13,40) MIN,SEC,VX,VY,VZ 
ENDIF 


PROCESS ACCELERATION DATA 
IF ( .NOT. ENDACCREC ) THEN 
READ(4,20,END=103) AX,AY,AZ 
WRITE(14,40) MIN,SEC,AX,AY,AZ 
ENDIF 
UPDATE COUNTERS AND TIMING 
PC=PC+1 
TOTSEC=TOTSEC+DELTAT 
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ххх 


ххх 


kkk 


ххх 


ххх 


ххх 


OQ O 02 


GOTO 100 
20 ҒОВМАТ(3(1Х,Е15.7)) 
30 ҒОВМАТ(1Х,16,2Х,3Ғ10.1,10Х,12,1Х,12,1Х,Е5.2) 
40 FORMAT(1X,I2,1X,F7.4,10X,F9.3,5X,F9.3,5X,F9.3) 


101 CONTINUE 
WRITE(*,121) PC-1 
ENDPOSREC = .TRUE. 
GOTO 100 
121 FORMAT( ' End of Data After ',I4,' Position Records 
*Read') 


102 CONTINUE 
WRITE(*,122) PC-1 
ENDVELREC = .ТЕЏЕ. 
GOTO 100 
122 FORMAT( ' End of Data After ',I4,' Velocity Records 
*Read') 


103 CONTINUE 
WRITE(*,123) PC-1 
ENDACCREC - .TRUE. 
GOTO 100 
123 FORMAT( ' End of Data After ',I4,' Acceleration Records 
*Read') 


200 CONTINUE 
CLOSE (1) 
CLOSE(2) 
CLOSE (3) 
CLOSE (4) 
CLOSE (12) 
CLOSE (13) 
CLOSE(14) 
CLOSE(15) 
STOP 
END 
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С. TORPMAN. FOR 


C--KALMAN FILTER---KALMAN SMOOTHING ALGORITHM-------------- 
C--THIS PROGRAM IS DESIGNED TO PROCESS 3/D DATA FILES FROM 
C--THE UNDERSEA TRACKING RANGE AT KEYPORT WA. A KALMAN 
C--FILTER IS APPLIED TO THE TRACK DATA WHICH CONSISTS OF 
C--x,Y, AND Z COORDINATES, AS WELL AS VELOCITY COMPONENT IN 
С--Х,Ү, AND Z COORDINATES. THEN, A KALMAN FILTER SMOOTHING 
C--ROUTINE GENERATES SMOOTHED POINTS IN X,Y, AND Z. THE 
C--PROGRAM GENERATES OUTPUT FILES WHICH CONTAIN THE VARIANCE 
C--OF THE X ESTIMATE VS SAMPLE FOR BOTH THE FORWARD KALMAN 
C--FILTER CASE AND THE KALMAN SMOOTHED CASE. FILES ARE ALSO 
C--GENERATED WHICH CONTAIN THE FILTERED X,Y,AND Z ESTIMATES 
C--AND THE SMOOTHED X,Y, AND Z ESTIMATES. THIS PROGRAM IS 
C--DESIGNED TO RUN ON THE IBM/AT PERSONAL COMPUTER BUT DUE 
C--TO THE SIZE OF THE DATA SETS INVOLVED, PLOTTING CANNOT BE 
C--DONE WITH THIS PROGRAM. PLOTTING OF OUTPUT DATA IS 
C--DONE USING MATLAB. THE PROGRAM GIVES THE USER THE OPTION 
C--OF CHANGING THE STARTING TIMES FOR BOTH THE RANGE AND 
C--VELOCITY DATA, THE VALUE OF THE INITIAL COVARIANCE 
C--MATRIX, AND THE EXCITATION PROCESS VECTOR. THE USER IS 
C--ALSO REQUIRED TO PROVIDE THE NAMES OF THE INPUT AND 
C--OUTPUT DATA FILES, AND THE NUMBER OF POINTS TO CALCULATE 
C--WITH A MAXIMUM OF 2500. THE PROGRAM USES A LARGE AMOUNT 
C--OF HARD DISK SPACE FOR TEMPORARY FILES (APPROXIMATELY 
C--160K BYTES/100 POINTS, 4MEG BYTES MAX FOR 2500 POINTS). 


e 
Qe ke ke ke kk ck ck ke ke ke ke kk kk kk kkkkk SECTION 1 *****kkkkkkkkkkkkkkkkkk 


C 
C kkkkkkkkkkkkkkk DECLARATION OF PARAMETERS & k k k k k k k k k * 
= 
INTEGER ONE 
PARAMETER ( ONE=1, MAXOBS=250 , NDIMEN=3 ) 
PARAMETER ( NUMSTA=3*NDIMEN, NSTMAT=NUMSTA*NUMSTA, 
ж NOBVEC=NUMSTA*MAXOBS ) 
С 
C kkkkkkkkkkkkkxkk DECLARATION OF ARRAYS kkkkkkkkkkkk 
Е 
DOUBLE PRECISION XKK (NUMSTA) , XKKM1 (NUMSTA) , 
* ZZ(NUMSTA) ZKKM1 (NUMSTA) , XNNM1 (NUMSTA) , XP1(NUMSTA) 
DOUBLE PRECISION TMPV1(NUMSTA) , TMPV2 (NUMSTA) 
DOUBLE PRECISION VTIME (МАХОВ5) , SOURCE (MAXOBS), 
ж ROLL (MAXOBS) 
DOUBLE PRECISION XKKS (NUMSTA , MAXOBS) 
С 
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00000000 о 


ОООООООООООООООООО 


DOUBLE PRECISION PHI (NUMSTA, NUMSTA) , 
Q(NUMSTA, NUMSTA) , HI (NUMSTA, NUMSTA) , 
PKKM1 (NUMSTA , NUMSTA) , PKK(NUMSTA , NUMSTA) , 
H (NUMSTA , NUMSTA) , HTRANS (NUMSTA , NUMSTA) , 
R(NUMSTA, NUMSTA) , G(NUMSTA , NUMSTA) , 
PHIT (NUMSTA , NUMSTA) , PKKS (NUMSTA, NUMSTA) , 
PKKP1 (NUMSTA, NUMSTA) , AK(NUMSTA, NUMSTA) , 
RPROJ (NUMSTA, NUMSTA) 

DOUBLE PRECISION TEMP1(NUMSTA, NUMSTA) , 

* TEMP2 (NUMSTA , NUMSTA) , TEMP3 (NUMSTA , NUMSTA) 


+ Xx X OR OR OF 


* k k k k * k k * * * DECLARATION OF SCALARS * k k k k k k k k k k k * * * 
DOUBLE PRECISION ACCTHRSH,ROLTHRSH,WTMIN,WTMAX 
COMMON /CBLK/ PTC(MAXOBS) , XYRANGE(3,2), 
* PKKONEONE (MAXOBS) , IR1 
INTEGER KF,SRC 


CHARACTER PKKS3D*13, PKKM1S*13 
CHARACTER ASK*1, INFILC*13, INFILR*13, INFILP*13, 


* INFILA*13, OUTFIL*13 
kk ek INITIALIZATION OF SELECTED ARRAYS  3*xxdxdkx 
kk ok (THOSE NOT OTHERWISE INITIALIZED) **Xxdxdxdkdkx 


DATA HI /NSTMAT*0.DO/, VTIME /MAXOBS*0.D0/, 
* XKKS /NOBVEC*0.D0/, SOURCE /MAXOBS*0.D0/, 
*  PKKM1 /NSTMAT*0.DO/, ROLL /MAXOBS*0.D0/ 


IIIIIIITIIIIIIIIIIIIIIGEE-JegNO) BPEEREIIZIIIIIIIIIIIIIIIIITIII: 
***** DESIGNATE AND OPEN INPUT AND OUTPUT FILES  *******x 


WRITE(*,*) 'ENTER NAME OF INPUT RANGE POSITION DATA 
*FILE! 

READ(50,'(A)') INFILP 

WRITE(*,*) 'ENTER NAME OF INPUT INTERNAL VELOCITY DATA 
*FILE! 

READ(51,'(A)') INFILA 

WRITE(*,*) 'ENTER NAME OF INPUT INTERNAL ACCELERATION 
«БАТА FILE! 

READ(52,'(A)') INFILC 

WRITE(*,*) 'ENTER NAME OF INPUT ROLL ANGLE DATA FILE! 
READ(53,'(A)') INFILR 

WRITE(*,*) "ENTER NAME OF SMOOTHED DATA FILE! 
READ(54,'(A)') OUTFIL 

OPEN(2,FILE= 'DATA\XKKFWD.DAT', STATUS='NEW') 
OPEN(3,FILE= 'DATA\XKSMOOTH.DAT', STATUS='NEW') 
OPEN(4,FILE= 'DATA\PKKOUT.DAT', STATUS='NEW') 
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000000000000 


OPEN(11,FILE-'DATANOBSERVAT.DAT' , STATUS='OLD' ) 


PKKS3D - 'DATAXS3DTEMP' 

PKKM1S - 'DATAXSMITEMP' 

OPEN(63,FILE-PKKS3D, ACCESS-'DIRECT', 

ж STATUS= 'NEW',FORM-'FORMATTED',RECL-135) 
OPEN(61,FILE-PKKM1S, ACCESS-'DIRECT', 

k STATUS= 'NEW!, FORM='FORMATTED' , RECL=135) 


* k k * * * k k * INITIALIZE SELECTED COUNTERS * * * k ЖК 


IR1 
IR 
K 


I 
1 
0 


k k * * INITIALIZE ARRAY DIMENSIONS AND VARIABLES  ******x 


L = 1 

N 29 

NW = 3 

ND = NUMSTA 
LD = ONE 


PKKM1(1,1) = 1000000.0D0 
ROLTHRSH-5.0 
ACCTHRSH-5.0 

WTMIN-1800. 

МТМАХ = 20000.0 


kkkk* INITIALIZE THE IDENTITY MATRIX AND X(1]0) ****** 


DO 190 I = 1, 9 
XKKM1(I) = 0.0 
HI(I,I) = 1. 

190 CONTINUE 


ххх} ECHO INITIAL FILTER PARAMETERS AND ALLOW * * * * * 
k * * * * USER TO CHANGE THEM IF DESIRED. * k * kx x 


CALL CHANGE(PKKM1,WTMIN,WTMAX,N) 
CALL SETTHRSH(ROLTHRSH,ACCTHRSH) 


kkkkkkkkkkkkkkkkkkkkkk SECTION 3 *<#*#*##*#** Q| +s e e e e e e 


**** READ IN THE "RAW" OBSERVATION DATA FILE, WHICH  *** 
**** HAS BEEN PROPERLY FORMATTED IN AN EARLIER хх 
**** E.G. OBSDATA.FOR. WHEN THIS DATA IS READ IN kk 
**** THE Kth OBSERVATION IS ORIGINALLY LOADED INTO  *x** 
****  XKKS(*,K). THEN, WHEN THE FILTER IS RUN, THIS *** 
**** VALUE WILL BE REPLACED WITH THE ESTIMATED STATE *** 
**** X(K!K). FINALLY, WHEN THE BACKWARD SMOOTHING IS *** 


63 


C *#Y>k* RUN, THIS VALUE WILL BE REPLACED WITH THE FINAL *** 
C ***** SMOOTHED VALUE. kkk 
С 
READ(11,210) IR1 
IF ( ТЕ .GT. MAXOBS ) THEN 
WRITE(*,209) ІКІ, МАХОВ5 
STOP 
ENDIF 
C 
DO 201 IPRT = 1,IR1 
READ(11,211,END=300) PTC(IPRT),VTIME(IPRT), 
* SOURCE(IPRT),( XKKS(JPRT,IPRT),JPRT-1,9), 
ж ROLL(IPRT) 
201 CONTINUE 
CLOSE(11) 
С 
209 FORMAT(' The Number of Observations ',I5,', exceeds 
*the',' maximum storage available (',I5,')' ) 
210 FORMAT(/,1X,14,/) 
211 FORMAT(13(1X,E14.8)) 
300 CONTINUE 
C 
C 
C * K k k k k k k k k k k k k k k kk kk* SECTION 4 ****** ke ke x x xx xx kx ke ke kx kx kx kx k kk kk 
С 
С ХХХХАХХХХХХХХХХХХХХХХЖХХХХХХХХХЖХХХХХХХХХХХХХХХХХАХХХХХАХЖЯХЭХ 
C *x**x IMPLEMENT THE (FORWARD) KALMAN FILTER * * k К 
С ХХХХАХАХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХЖХХХХХХХХХХХХХХХХХХХ 
C 


100 K = K + 1 
WRITE(*,*) 'Forward Filter - Step',K 


Е 
С *** BYPASS COMPUTATION OF P(1!0) AND X(1!0) LI. 
C *** DURING FIRST ITERATION ak 
C 
IF (K.EQ.1) GO TO 101 

C 
С *** GENERATE THE APPROPRIATE STATE TRANSITION dk 
С *** MATRIX ( PHI ) AND ITS TRANSPOSE, BASED ON *** 
C *** THE ELAPSED TIME SINCE THE LAST OBSERVATION.*** 
Е 

DT = УТІМЕ(К) -УТІМЕ (К-1) 

CALL PHIDEL(DT,NW,PHI,ND) 

CALL TRANS(PHI,N,N,PHIT,ND, ND) 
Ç 
C **** DETERMINE WHETHER THE FILTER BELIEVES THE **** 
C **** TORPEDO IS MANEUVERING, AND GENERATE THE LII 
C **** APPROPRIATE COVARIANCE OF EXCITATION MATRIX **** 
С 


CALL MANDET (ROLL, XKKS, K, ROLTHRSH, ACCTHRSH, WIMIN, 
* WTMAX,DT,Q,ND,NW) 
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00000000 


000000 


O O 


(3000000 


оосо (C6) 0) 


(30000 


(00000 


101 


G(K) 


*** 


k kk 
ххх 


ххх 
ххх 
kkk 


FORM P(K|K-1), BASED ON THE EQUATION 


P (Kj K-1) PHI*P(K-1lK-1)*TRANS(PHI) -* Q(K) 
..WHERE  P(K-1|K-1) 
PREVIOUS STEP. 


IS Р(КІК) FROM THE 


CALL PROD(PKK,PHIT,N,N,N,TEMP1,ND,ND,ND) 
CALL PROD(PHI,TEMP1,N,N,N,TEMP2,ND,ND,ND) 
CALL ADD(TEMP2,Q,N,N, PKKM1,ND,ND) 


AND ALSO X(K!K-1) 
( WHERE X(K-1|K-1) 


PREVIOUS STEP.) 


PHI*X(K-1|K-1) 
IS X(K|K) FROM THE 


CALL PROD(PHI,XKK,N,N,ONE, XKKM1,ND,ND, ONE) 


CONTINUE 

**** DETERMINE THE SOURCE OF THIS OBSERVATION 
**** (E.G. POSITION ONLY, VELOCITY ONLY, 

**** POSITION AND VELOCITY, ETC.) AND FORM THE 
kkkk CORRESPONDING MEASUREMENT MATRIX ( H(K) ) 
**** AND ITS TRANSPOSE 

SRC = SOURCE (K) 


CALL BUILDH(SRC,M,H,NW,ND) 
CALL TRANS (H,M,N,HTRANS, ND, ND) 


ххх 
kkk 
kkk 
ххх 


DETERMINE THE FULL (RANGE-DEPENDENT) 
COVARIANCE OF MEASUREMENT NOISE MATRIX ( R ) 
THEN PROJECT IT ONTO THE CURRENT OBSERVATION 
VECTOR COMPONENTS 


CALL BUILDR(R,NW,ND) 
CALL PROD(H,R,M,N,N,TEMP1,ND,ND,ND) 
CALL PROD(TEMP1,HTRANS,M,N,M,RPROJ,ND,ND,ND) 


* k k * 
KKK 
k kkk 


USE THE MEASUREMENT MATRIX ( H ) TO SELECT 
THE CORRECT COMPONENTS ( Z(K) ) FROM THE 
"RAW" OBSERVATION ARRAY ( XKKS ). 


CALL PROD(H,XKKS(1,K),M,ND,ONE,ZZ,ND,ND, ONE) 


**k*k*k 
**k 


COMPUTE THE OPTIMUM GAIN MATRIX G(K) 
BASED ON THE FORMULA 
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k*k 


ЖЖЖ 
ххх 


kkk 
kkk 
* k 


k kkk 
* K * k 
* * x 
k kkk 
k kkk 


ххх 
ххх 
kkk 
kkk 


k k kk 
k kkk 
k kkk 


RK 
k kkk 


AAAAAA ANNA 


ОООООО 


Q00000 


Q 00 


P(K!K-1)*TRANS(H(K) ) XINV((H(K) *P(K| K-1) *TRANS(H(K)) * R]) 


50 


602 
800 
900 


kkekxk 


CALL 
CALL 
CALL 
CALL 
CALL 


*ckok 
*ckck 


CALL 


* k xw 
ЖЖЖЖ 
*kkk 
* k 


CALL 
CALL 
CALL 


kkk 
* xx 


CALL 
CALL 
CALL 


kkk 
kkk 
kkk 


PROD(PKKM1,HTRANS,N,N,M,TEMP1,ND,ND, ND) 
PROD(H,TEMP1,M,N,M,TEMP2,ND,ND, ND) 
ADD(TEMP2,RPROJ,M,M, TEMP3 , ND, ND) 
RECIP(TEMP3, TEMP2,M, ND) 
PROD(TEMP1,TEMP2,N,M,M,G,ND, ND, ND) 


CALCULATE 
2 (К|К-1) 


2(КІК-1) BASED ON THE FORMULA 
H(K) *X(K|K-1) 


PROD(H,XKKM1,N,N,ONE,ZKKM1,ND, ND, ONE) 


SOLVE FOR THE UPDATED STATE ESTIMATE VECTOR 
( Х(КІК) ), BASED ON THE EQUATION 

X (Kl K) X(KlK-1) + G(K)*[Z(K) - Z(KlIK-1)] 
WHERE Z(K) IS THE CURRENT OBSERVATION 


SUB(ZZ,ZKKM1,M,ONE, TMPV1,ND,ONE) 
PROD(G,TMPV1,N,M,ONE, TMPV2,ND, ND, ONE) 
ADD(XKKM1,TMPV2,N,ONE, XKK, ND, ONE) 


NOW COMPUTE THE COVARIANCE OR ERROR ESTIMATE 
MATRIX Р(К\К) , BASED ON THE EQUATION: 


P(K|K) = ( I - G(K)*H(K) )*P(K[K-1) 
PROD(G,H,N,M,N,TEMP1,ND,ND,ND) 
SUB(HI,TEMP1,N,N,TEMP2,ND,ND) 
PROD(TEMP2,PKKM1,N,N,N,PKK,ND,ND, ND) 


STORE THE FILTERED DATA IN XKKS(*,K) (FOR 
SUBSEQUENT SMOOTHING (BACKWARDS FILTERING) 
AND ALSO SAVE X(K!K), P(K!K), AND P(K!K-1) 


DO 50 I=1,9 


XKKS (I, K) 


WRITE(2,602) VTIME(K),XKKS(1,K), 


- ХКК(І) 


XKKS(4,K), 


МЕТТЕ (4,900) РКК(1,1),РКК(1,4),РКК(4,1),РКК(4,4) 


KREC = 9*(K-1)+1 
WRITE(61,800,REC-KREC) PKKM1 
WRITE(63,800,REC=KREC) PKK 


FORMAT (4(1X,E14.8)) 
FORMAT (9(1X,D14.8)) 
FORMAT (9 (1X,E14.8)) 


kek 
k kkk 


ЖЖЖЖ 
* k xx 
* k xx 
* x 


ЖЖЖ 
kkk 


хаҡ 
kkk 
k kk 


ХКК5(7,К) 


AND CONTINUE UNTIL ALL OBSERVATIONS ARE FILTERED **** 


IF (K.LT.IR1) GOTO 100 
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CLOSE (2) 

CLOSE (4) 

CLOSE (61) 

CLOSE (63) 
С 
С 
Qe e eec e e e e e de ke e ke kk kx kx kk kk SECTION 5 ***o ee kk e e ke ke e e de e oe e ke e kn kk Ж Ж 
C 
OeMEZEXZEZEEIEIZEXZEREREREREEREREEREREEERERESZEEZEREREEEREERZERERERERE: 
С ж IMPLEMENT THE SMOOTHING (BACKWARD FILTERING) ROUTINE * 
C kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 
С 
С 
С ***** ОРЕМ THE REQUIRED DATA FILES, AND INITIALIZE **** 
C ***** THE SMOOTHED COVARIANCE OF ERROR ESTIMATE kkk 
С **w*** MATRIX ТО P(IR1[IR1) oe 
e 

OPEN(63,FILE-PKKS3D,ACCESS-'DIRECT',STATUS-'OLD', 

* FORM- ' FORMATTED' , RECL=135) 

OPEN(61,FILE-PKKM1S,ACCESS-'DIRECT',STATUS-'OLD', 

* FORM=' FORMATTED! , RECL=135) 


KF=9* (IR1-1)+1 
READ(63,800,REC=KF) PKKS 


С 
C ***k*k* THEN BEGIN THE BACKWARD SMOOTHING * * k * * * * * 
C 

DO 600 K=1,IR1 - 1 

KI= IR1 - K ° 

WRITE(*,*) 'Backward Smoothing - Step ',KI 
© 
C ***** GENERATE PHI MATRIX AND RETRIEVE THE STORED ЖЖЖ 
C ***** VALUES OF P(K,K), AND Р(К+1!К) * 
C 

DT = VTIME(KI+1) - VTIME(KI) 

CALL PHIDEL(DT, NW, PHI,ND) 
Ç 
e 

КЕ= 1+9%(К1-1) 

READ(63,800,REC-KF) РКК5 

READ(61,800,REC=KF+9) РККР1 
= 
C **** NOW COMPUTE THE SMOOTHING MATRIX, BASED ON THE **** 
C **** EQUATION: =1 kkk 
C **** А(К) = P(K!K) *TRANS (PHI) *P(K+1!K) kk 
С 


CALL TRANS(PHI,N,N,PHIT,ND, ND) š 
CALL RECIP(PKKP1,TEMP1,N,ND) | 
CALL PROD(PHIT,TEMP1,N,N,N,TEMP2,ND,ND,ND) — 
CALL PROD(PKKS,TEMP2,N,N,N,AK,ND,ND,ND) 
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000 


00000020 


000000 


000 


e 
C 


kk kk SOLVE FOR SMOOTHED ESTIMATE X 
* k k k * k XS(K) = Х(КІК) % А(К)%(Х5(К%1) - Х(Кғ11К)) kk 


DO 504 I = 1,9 
XP1(I) = XKKS(I,KI) 
XNNM1(I) = XKKS(I,KI+1) 
504 CONTINUE 
CALL PROD(PHI,XP1,N,N,ONE,TMPV1,ND,ND, ONE) 
CALL SUB(XNNM1,TMPV1,N,ONE,TMPV2,ND,ONE) 
CALL PROD(AK,TMPV2,N,N,ONE, TMPV1,ND,ND, ONE) 
CALL ADD(XP1,TMPV1,N,ONE,TMPV2,ND,ONE) 
DO 505 I = 1,9 
XKKS(I,KI) = TMPV2(I) 
505 CONTINUE 


***** AND UPDATE THE SMOOTHED COVARIANCE OF ERROR kk ke 
***** ESTIMATE MATRIX BASED ON THE EQUATION kk 
k k k k k PS(KIK) = P(KlK) 4 ЖҰЖ Ж 
* k kk * A(K)*(PS(K+1]K+1) - P(K+1l1!K)]*TRANS(A(K)) **x** 


CALL SUB(PKKS,PKKP1,N,N,TEMP1,ND,ND) 

CALL TRANS(AK,N,N,TEMP2,ND,ND) 

CALL PROD(TEMP1,TEMP2,N,N,N,TEMP3,ND,ND, ND) 
CALL PROD(AK,TEMP3,N,N,N,TEMP1,ND,ND,ND) 
CALL ADD(PKKS,TEMP1,N,N,PKKS,ND,ND) 


***** AND CONTINUE UNTIL ALL RECORDS ARE SMOOTHED ЖЖЖЖ 
600 CONTINUE 


ЖЖЖЖ SAVE THE SMOOTHED DATA kK 


DO 601 KI = 1,IR1 
WRITE(3,602) VTIME(KI),XKKS(1,KI),XKKS(4,KI), 
k XKKS (7, KI) 
601 CONTINUE 


жк CLOSE ALL FILES AND EXIT dk 
CLOSE(3) 
CLOSE(63,STATUS-'DELETE!) 
CLOSE(61,STATUS-'DELETE!) 


STOP 
END 


Chkkkkkkkkkkkkkkkkkkkkkk SECTION б ХХХХЕЕЕЕХЕЕЖХХЕХХЕ ХХХ К 


С 
С 
С 


k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k 


E REQUIRED  SUBROUTINES ** 
k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k ke e kk kk 
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С) 


Q00000 


OQ 


ХХХХҰХХҰХХХХХХХХ ХХҰХХХЖХХХХХХХХХХХХЕХХЖХХХХХХХХХХХХАХАХХЖХХ ХХХ ХЖАХХЖ 
SUBROUTINE WHICH COMPUTES THE PHI MATRIX 
THE MATRIX IS BUILD OF SUCCESSIVE BLOCKS, ONE 
PER COMPONENT DIRECTION OBSERVED, WHERE EACH 
BLOCK HAS THE FORM: 
ПИЕ NET^2/2 | 


i 

lo: x 5 

' oO O 1 | 
THIS PROCEDURE ASSUMES THE COMPONENTS OF THE "RAW" 
STATE VECTOR ARE T 


ЖУ ҮІ 22! 227222. 
k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k 


SUBROUTINE PHIDEL(T,NW,PHI,ND) 
DOUBLE PRECISION PHI(ND,ND) 


IF ( 3*NW .GT. ND ) THEN 
WRITE(*,201) NW, ND 
STOP 

ENDIF 


DO 100 IR = 1,NW 
IBLK = 3*(IR-1) 
PHI (IBLK+1, IBLK+1) 
PHI (IBLK+1, IBLK+2) 
PHI (IBLK+1, IBLK+3) 
PHI (IBLK+2, IBLK+2) 
PHI (IBLK+2, IBLK+3) 

100 PHI (IBLK+3, IBLK+3) 
RETURN 


l Hl H H W I 
ЕЗ 
° * е 
rj 
hos 
N 
О 
o 


201 FORMAT(' **** ERROR IN SUBROUTINE PHIDEL - 3*NW 
* (-2',I13,') ',' .GT. ND (-',13,') ') 
END 


X EXZEEREREEREREREEREREZEREREEREEEREREREERERRERREREERERERERERERERE: 


SUBROUTINE WHICH ADDS TWO INPUT MATRICES 
kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 


SUBROUTINE ADD(A,B,N,M,C,ND,MD) 
DOUBLE PRECISION A(ND,MD),B(ND,MD),C(ND,MD) 


IF ( ( N .GT. ND ) .OR. ( M .GT. MD) ) THEN 
WRITE(*,201) N, ND, M, MD 
STOP 

ENDIF 


DO 100 I 


= 1,N 
DO 100 J = 1 


,М 
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00000 O 


O 


100 C(I J) = AUD J) B(T J) 


RETURN 
201 FORMAT(' **** ERROR IN SUBROUTINE ADD - ',/,10X 
* 'Either N (5',13,')',' „ет. не. (=" а MU 
* ' or M (= " ЕЗУ . GT. MD (=*, 13 075) 
END 


* k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k kk k kk k k k kk k k k k k kk kk 


SUBROUTINE WHICH SUBTRACTS THE SECOND INPUT MATRIX FROM 


THE FIRST 
K * k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k * 


SUBROUTINE SUB(A,B,N,M,C,ND,MD) 
DOUBLE PRECISION A(ND,MD),B(ND,MD),C(ND,MD) 


IF ( ( N .GT. ND ) .OR. ( M.GT. MD) ) THEN 
WRITE(*,201) N, ND, M, MD 


STOP 
ENDIF 
DO 100 I = 1,N 
DO 100 J = 1,M 
100 C(I,J) - А(І,2) - В(І,7) 
RETURN 
201 FORMAT(' **** ERROR IN SUBROUTINE SUB - ',/,10X 
* 'Either N (2',13,')',' .GT. ND (=',13,')',/,10X, 
ж ' or M (2',13,')',' .GT. MD (=',13,')') 
END 


* k k k k k k k k * k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k kk kk k 


SUBROUTINE WHICH MULTIPLIES TWO INPUT MATRICES 
k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k 


SUBROUTINE PROD(A,B,N,M,L,C,ND,MD,LD) 
DOUBLE PRECISION A(ND,MD),B(MD,LD),C(ND,LD) 


IF ( ( N .GT. ND ) .OR. ( M .GT. MD ) .OR. 
( L.GT. LD) ) THEN 
WRITE(*,201) N, ND, M, MD, L, LD 


STOP 
ENDIF 
DO 100 I - 1,М 
DO 100 J = 1,L 
C(I,J) = 0.DO 
DO 100 K = 1,M 
C(I,J) =C(I,J) + A(I,K)*B(K,J) 
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000000 


о 


Oo Oo ooon 


100 CONTINUE 


RETURN 
201 FORMAT(' **** ERROR IN SUBROUTINE PROD ~ ',/,10X 
* 'Either N (=',I3,')!',' „ст. ND (=, Dp xu 7, 10X, 
* QE MENS UP! СТ. MD (s',13,')',/,10X, 
ж ' or та „от... LD (=, тз уш) 
END 
kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 
SUBROUTINE WHICH COMPUTES THE TRANSPOSE OF THE INPUT 
MATRIX 
kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 
SUBROUTINE TRANS(A,N,M,C,ND,MD) 
DOUBLE PRECISION A(ND,MD),C(MD,ND) 
IF ( ( N .GT. ND) .OR. ( M .GT. MD) ) THEN 
WRITE(*,201) N, ND, M, MD 
STOP 
ENDIF 
DO 100 I = 1,N 
- DO-100 J = 1,M 
C(J,I) = А(Т,Ј) 
100 CONTINUE 
RETURN 
201 FORMAT('! **** ERROR IN SUBROUTINE TRANS - ',/,10X, 
* па песа (= ава |)“ “ст. мо (=",13,")",/,10Х, 
* ' Or MO Tom Gr. MD (=',I3,')"') 
END 
ХХХ ХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХХЖАХЭХЖ 
SUBROUTINE WHICH CALCULATES THE INVERSE OF THE INPUT 
MATRIX 
kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 
SUBROUTINE RECIP(A,C,N,ND) 
DOUBLE PRECISION A(ND,ND),C(ND,ND),D(20,20), TEMP 
IF ( N .GT. ND) THEN 
WRITE(*,801) N, ND 
STOP 
ENDIF 


DO 100 I - 1,N 
DO 100 J - 1,N 
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000000000 


100 D(1 3) = АО) 
DO 115 I - 1,N 
DO 115 J = N*1,2*N 
115 D(I,J) = 0.0 


DO 120 I = 1,N 
J=I+N 
120. D(I,J) = 1.0 


DO 240 K= 1,N 


M = K+ 1 
IF (K .EQ. N) GOTO 180 
L = K 


DO 140 I = M,N 
140 ТЕ (ABS(D(I,K)) .GT. ABS(D(L,K))) L = I 
IF (L .EQ. K) GOTO 180 
DO 160 J = K,2*N 
TEMP = D(K,J) 
D(K,J) = D(L,J) 
160 D(L,J) = TEMP 
180 DO 185 J = М,2%М 
185 D(K,J) = D(K,J)/D(K,K) 
IF (K .EQ. 1) GOTO 220 
Ml = K - 1 
DO 200 I = 1,M1 
DO 200 J = M,2*N 
200 D(I,J) = D(I,J) - D(I,K)*D(K,J) 


IF (K .EQ. N) GOTO 260 
220 DO 240 I =M,N 
DO 240 J = M,2*N 
240 D(I,J) = D(I,J) - D(I,K)*D(K,J) 
260 DO 265 I = 1,N 
DO 265 J = 1,N 


K= J +N 
265 C(I,J) = D(I,K) 
RETURN 


801 FORMAT(' **** ERROR IN SUBROUTINE RECIP - N 
*(7',I3,')','! .GT. ND (-',13,') ') 
END 


K * k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k 
SUBROUTINE WHICH ALLOWS THE USER TO CHANGE W AND PKKM1 
TO ALTER THE FILTER BEHAVIOR WITHOUT HAVING TO RECOMPILE 


THE PROGRAM 
kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 
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SUBROUTINE CHANGE(PKKM1,WTMIN,WTMAX,ND) 
DOUBLE PRECISION PKKM1(ND,ND) 
REAL*8 X,WTMIN,WTMAX 


WRITE(*,21) PKKM1(1,1) 
WRITE(*,22) 
READ(*,23,END=24) X 
IF ( X .LE. 0. ) GOTO 24 
PKKM1(1,1) = X 
WRITE(*,25) PKKM1(1,1) 
24 CONTINUE 
DO 26 I=1,9 
PKKM1(I,I)=PKKM1(1,1) 
26 CONTINUE 


21 FORMAT(/,' The Covariance of Error Estimate Matrix - 
,'currently has the value:',//,10x,'P(I,I) - 
,F14.4,//,' A large value (approx. 10**6) will 

produce ','a fast initial response.',/, 

'Decreasing one or more decades from this value 

will ','slow the initial response!) 

22 FORMAT(' You may simply hit "Enter" to accept this 

value,',', or',/,' enter a new value now (being 
* sure to include the decimal point) ') 

23 FORMAT (D14.5) 

25 FORMAT(' Covariance of Error Estimate Matrix reset 
* to',' - ',p14.5) 


+ + + + * 


* 


WRITE(*,900) WTMIN 
WRITE(*,22) 
READ(*,23,END=910) X 
IF ( X .LE. 0. ) GOTO 910 
WIMIN = X 
WRITE(*,901) WTMIN 
900 FORMAT(/,' The current MINIMUM weight on the Q matrix 


* for a ','maneuver is: ',F9.3) 
901 FORMAT(' MINIMUM weight on the Q matrix for a ', 
* ‘maneuver reset to: ',F9.3) 


910 WRITE(*,905) WTMAX 
WRITE (*, 22) 
READ (*,23,END=920) X 
IF ( X .LE. O ) GOTO 920 
ЯТМАХ - Х 
WRITE(*,906) WIMAX 
920 CONTINUE 5 
905 FORMAT(/,' The current UE weight on the Q matrix 


* for a ','maneuver is: ',F9.3) 
906 SEORSA MAXIMUM weight on the’ Ота о=1х Тос а ', 
‘maneuver reset to: ',F9.3) 
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03000020 


0000000000000 


RETURN 
END 


* k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k kk k 


SUBROUTINE WHICH ALLOWS USER TO CHANGE THRESHOLDS 
k k ke k k ke ke ke ke ke ke k k ke k k k k k k k k k k k k k k k k * k k k k k k k k k k * * k k k k k k k k k k k k k 


SUBROUTINE SETTHRSH(ATHRSH,RTHRSH) 
REAL*8 ATHRSH,RTHRSH,TMP 


WRITE(*,10) ATHRSH 
WRITE(*,22) 
READ(*,50,END-25) TMP 
IF ( TMP .LE. 0. ) GOTO 25 
ATHRSH = TMP 
WRITE(*,11) ATHRSH 
10 FORMAT(/,' The current Acceleration Threshold 
* is:',F8.3) 
11 FORMAT(' Acceleration Threshold reset to:',F8.3) 
22 FORMAT(' You may simply hit "Enter" to accept this 


* value,', 
* ', Or',/,' enter a new Value now (being sure to 
* include the decimal point)') 


50 FORMAT (D12.3) 


25 WRITE(*,30) RTHRSH 
WRITE(*,22) 
READ(*,50,END-40) TMP 
IF ( TMP .LE. 0.0 ) СОТО 40 
RTHRSH = TMP 
WRITE(*,31) RTHRSH 
30 FORMAT(/,' The current Roll Threshold is:',F8.3) 
31 FORMAT(' Roll Threshold reset to:',F8.3) 


40 CONTINUE 
RETURN 
END 


k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k kk k kk kk 

SUBROUTINE TO DETECT IF A MANEUVER IS OCCURRING AND SET Q 
APPROPRIATELY. THIS SUBROUTINE ASSUMES THE COMPONENTS OF 

THE "RAW" STATE VECTOR ARE 


T 
[X x' x" Y y' yn g 720070 E 


AND BUILDS THE Q MATRIX BLOCK BY BLOCK, ACCORDING TO 
THE PATTERN: 
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O 


о 00000000000 


Q(1,1) — (DT**6/36) * WI 
Q(2,2) = (DT**4/4) * WT 
Q(3,3) = DT**2 * WT 


BUT CHANGING THE WEIGHTS IN EACH BLOCK ACCORDING TO 
WHETHER THE TORPEDO APPEARS TO BE MANEUVERING IN THAT 


DIRECTION. 
k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k e e e e e e e e e ee ee Ж 


SUBROUTINE MANDET(RL,ZK,K,RTH,ATH,WTMIN,WTMAX, 
* DT,Q,ND,NW) 


DOUBLE PRECISION RL(1),ZK(ND,1),Q(ND,ND) 
DOUBLE PRECISION RTH,ATH,WTMIN,WTMAX,WT 
INTEGER K,ND,NW 


IF ( 3*NW .GT. ND) THEN 

WRITE(*,201) 3*NW, ND 

STOP 
ENDIF 
DO 100 ICOM-1,NW 

WT-WTMIN 

IF( ( ABS(RL(K)) .GT.RTH ) .OR. 
* ( ABS(ZK(3*ICOM,K)) .GT. ATH ) ) WT=WTMAX 


IBLK = 3*ICOM 
Q(IBLK-2,IBLK-2) 
Q(IBLK-1,IBLK-1) 
Q(IBLK,IBLK) 

100 CONTINUE 


(DT**6)*WT/36.DO0 
(DT**4)*WT/4.DO 
(DT**2) *WT 


RETURN 
201 FORMAT(' #**** ERROR IN SUBROUTINE MANDET - 3*NW 
* айна ст. рз) 
ЕМО 


* k k k k k k 2 * k k k k k k k k k k k k k k k k k k k k k k k kk k k kk k k k kk k kk kk k kk kk k k kk : 


SUBROUTINE TO BUILD THE RANGE DEPENDENT COVARIANCE OF 
MEASUREMENT MATRIX - R (TBD) 


THIS SUBROUTINE ASSUMES THE COMPONENTS OF THE "RAW" 
STATE VECTOR ARE T 


NE O тал лы 2" , wis 
£ k * * * k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k e e e e de de ek eek ek o Ж 


SUBROUTINE BUILDR(R,NW,ND) 


DOUBLE PRECISION R(ND,ND) 
INTEGER NW 
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о ООООООООООООООО 


ООО 


IF ( 3*NW .GT. ND) THEN 
WRITE(*,201) 3*NW, ND 
STOP 

ENDIF 


kde ZERO OUT ANY PREVIOUS VALUES ka RR RK 
DO 90 IVAR = 1,ND 
DO 90 JVAR = 1,ND 
R(IVAR,JVAR) = 0.DO 
90 CONTINUE 


DO 100 IVAR = 1, NW 


R(3*IVAR-2,3*IVAR-2) = 15.0D0 
R(3*IVAR-1,3*IVAR-1) = .03D0 
R(3*IVAR,3*IVAR) = .09DO 
100 CONTINUE 
RETURN 
201 FORMAT(' **** ERROR IN SUBROUTINE BUILDR - 3*NW 
* 2!,13,!')',' .GT. ND (-',13,')!) 
END 


k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k 
SUBROUTINE TO BUILD THE MEASUREMENT MATRIX - H 


THIS SUBROUTINE ASSUMES THE COMPONENTS OF THE "RAW" 
STATE VECTOR ARE T 
[ X X' X" Y V UY 72 ОЕ E 


SRC IS INTERPRETED А5 А BINARY NUMBER, WHERE A "1" ІМ 
A GIVEN POSITION MEANS THAT COMPONENT IS ALSO PRESENT IN 
THE CORRESPONDING OBSERVATION, E.G. 

SRC = 23 = (000010111) (blnary) 


IMPLIES X(1),X(2),X(3) AND X(5) MAKE UP THE OBSERVATION. 
* k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k k kk k k kk k k k k k k k k k k k k 


SUBROUTINE BUILDH(SRC,M,H,NW,ND) 

DOUBLE PRECISION H(ND,ND) 

INTEGER NW,ND,SRC,M,IROW ` 

INTEGER BINCODE(9) /1,2,4,8,16,32,64,128,256/ 

IF ( ( 3*NW .GT. ND) .OR. ( ND .GT. 9 ) ) THEN 
WRITE(*,201) 3*NW, ND 
STOP 

ENDIF 
kdo ZERO OUT ANY PREVIOUS VALUES dk kk eek 


DO 90 IVAR=1,ND 
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DO 90 JVAR=1,ND 
H(IVAR,JVAR) = 0.р0 
90 CONTINUE 


ІКОМ - 0 


kkkkkkk INCLUDE OBSERVATIONS AS APPROPRIATE  ****x* 
kk***** BASED ON WHETHER THE CORRESPONDING  ***** 
kkkkkk*k BINARY DIGIT IS PRESENT IN SRC kkk KK 
DO 100 IVAR=1,ND 
IF ( MOD( SRC/BINCODE(IVAR) ,2) .EQ. 1) THEN 
` IROW = IROW + 1 
H( IROW,IVAR ) = 1.DO 
ENDIF 
100 CONTINUE 


0000 о 


С 
М = IROW 
RETURN 

Ç 

201 FORMAT(' **** ERROR IN SUBROUTINE BUILDH - 3*NW 

k -!,13,!)!,! .GT. ND (-',1I3,'), OR ND » 9!) 
END 

А7, 


TH 
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